import datetime
from PyQt5 import QtWidgets, QtGui
import tcp_udp_web_ui
import socket
import threading
import sys
import math
import time
import struct
import cv2
from PIL import Image
from io import BytesIO

import matplotlib.pyplot as plt

# 目标融合
import numpy as np
import queue

import re

import itertools
from collections import deque, OrderedDict
from datetime import datetime, date

# 定义雷达数据存放的容器
queue_radar_target = queue.Queue()  # 用于融合
queue_laser_target = queue.Queue()
mutex_radar = threading.Lock()  # 共享资源锁
mutex_sound = threading.Lock()  # 共享资源锁

Parameter_len_size = 38  #设置雷达

DEBUG = False
dict_image_format = {'01': 'yuv420', '02': 'yuv422', '03': 'yuv444', '10': 'bmp', '11': 'jepg', '12': 'png'}

# 时间配准
radar_sync_time_queue = deque(maxlen=10)  # 建立雷达的同步时间双边队列
sound_sync_time_queue = deque(maxlen=10)  # 建立声源的同步时间双边队列

collect_radar_data = deque(maxlen=20)  # 建立采集数据队列
sure_target_deque = deque(maxlen=20)  # 建立确信雷达目标队列
sure_target_possible = deque(maxlen=20)
noise_target_deque = deque(maxlen=10)  # 建立可能雷达目标队列，存储较为可能是雷达目标的数据
radar_target_precess = deque(maxlen=10)  # 建立雷达目标数据集合队列
radar_update = deque(maxlen=10)  # 建立通信雷达更新队列
radar_target_deque = deque(maxlen=10)  # 建立雷达目标数据集合队列
sum_radar_target = deque(maxlen=10)  # 建立储存最新的所有目标数目的队列
unregistered_radar = deque(maxlen=5)  # 建立雷达未同步双边队列
radar_data_queue = deque(maxlen=10)  # 建立同步后雷达双边队列

sound_aver_data = deque(maxlen=10)  # 建立采集数据队列
sound_target_precess = deque(maxlen=10)  # 建立雷达目标数据集合队列
sound_update = deque(maxlen=10)  # 建立通信声源更新队列
sound_target_deque = deque(maxlen=10)  # 建立雷达目标数据集合队列
sum_sound_target = deque(maxlen=10)  # 建立储存最新的所有目标数目的队列
unregistered_sound = deque(maxlen=10)  # 建立声源未同步双边队列
sound_data_queue = deque(maxlen=10)  # 建立同步后声源双边队列

radar_communication = queue.Queue()  # 通信生成的雷达队列
radar_filter_communication = deque(maxlen=10)  # 经过滤波后的雷达队列
sound_communication = queue.Queue()  # 通信生成的声源队列
sound_filter_communication = deque(maxlen=10)  # 经过滤波后的的声源队列


# 处理墙体数据
def bin2img(bin_data):
    img_data = np.frombuffer(bin_data, dtype=np.uint8).reshape(101, 611)
    #
    row = 101
    column = 611
    xarray = []
    yarray = []
    for i in range(0, row):
        for j in range(0, column):
            if (img_data[i][j] == 1):
                xarray.append(round((i - 51) * 0.2, 2))  # 保留两位小数
                yarray.append(round(j * 0.1, 2))
    return xarray, yarray


# 接受固定字节的数据
# data = RecvN(sockfd, 30 * 1024 + 14)
def RecvN(socket, n):
    totalContent = bytes(''.encode("utf8"))
    totalRecved = 0
    while totalRecved < n:
        onceContent = socket.recv(n - totalRecved)
        # print("onceContent", onceContent)
        totalContent += onceContent
        totalRecved = len(totalContent)

    return totalContent


# 雷达目标解析程序
def dataRadarAnalysis(data):
    start = 0
    object_list = []
    data_number_radar_dec = ''
    while start < len(data):
        if (data[start + 0:start + 5] == b'\xa5\x5a\xb5\x10\x30'):
            flag = 1
            data_length_hex = data[start + 5:start + 9]
            data_length_dec = struct.unpack('i', bytes(data_length_hex))[0]
            if DEBUG:
                print("目标参数长度", data_length_dec)

            if (((sum(data[2:-1]) & 0xff) == data[-1] ) | flag == 1):  # 假设校验位为
                data_number_hex = data[start + 9:start + 10]
                data_number_dec = struct.unpack('b', bytes(data_number_hex))[0]  #
                if DEBUG:
                    print("目标个数", data_number_dec)
                data_number_radar_dec = data_number_radar_dec + str(data_number_dec) + ';'
                Parameter_len = Parameter_len_size
                data_target = []
                for i in range(0, data_number_dec):
                    data_time_year_hex = data[start + 10 + i * Parameter_len:start + 12 + i * Parameter_len]
                    data_time_year_dec = struct.unpack('h', bytes(data_time_year_hex))[0]  #
                    if DEBUG:
                        print("年", data_time_year_dec)

                    data_time_month_hex = data[start + 12 + i * Parameter_len:start + 13 + i * Parameter_len]
                    data_time_month_dec = struct.unpack('b', bytes(data_time_month_hex))[0]  #
                    if DEBUG:
                        print("月", data_time_month_dec)

                    data_time_day_hex = data[start + 13 + i * Parameter_len:start + 14 + i * Parameter_len]
                    data_time_day_dec = struct.unpack('b', bytes(data_time_day_hex))[0]  #
                    if DEBUG:
                        print("日", data_time_day_dec)

                    data_time_hour_hex = data[start + 14 + i * Parameter_len:start + 15 + i * Parameter_len]
                    data_time_hour_dec = struct.unpack('b', bytes(data_time_hour_hex))[0]  #
                    if DEBUG:
                        print("时", data_time_hour_dec)

                    data_time_minute_hex = data[start + 15 + i * 30:start + 16 + i * 30]
                    data_time_minute_dec = struct.unpack('b', bytes(data_time_minute_hex))[0]  #
                    if DEBUG:
                        print("分", data_time_minute_dec)

                    data_time_second_hex = data[start + 16 + i * Parameter_len:start + 17 + i * Parameter_len]
                    data_time_second_dec = struct.unpack('b', bytes(data_time_second_hex))[0]  #
                    if DEBUG:
                        print("秒", data_time_second_dec)

                    data_time_millisecond_hex = data[start + 17 + i * Parameter_len:start + 19 + i * Parameter_len]
                    data_time_millisecond_dec = struct.unpack('h', bytes(data_time_millisecond_hex))[0]  #
                    if DEBUG:
                        print("毫秒", data_time_millisecond_dec)

                    data_target_state_hex = data[start + 19 + i * Parameter_len:start + 20 + i * Parameter_len]
                    data_target_state_dec = struct.unpack('b', bytes(data_target_state_hex))[0]  #
                    if DEBUG:
                        print("目标运动状态", data_target_state_dec)

                    data_target_strength_hex = data[start + 20 + i * Parameter_len:start + 24 + i * Parameter_len]
                    # data_target_strength_hex = data[22 + i * 26:24 + i * 26] + data[20 + i * 26:22 + i * 26]
                    data_target_strength_dec = struct.unpack('f', bytes(data_target_strength_hex))[0]  #
                    data_target_strength_dec = format(data_target_strength_dec, '.3f')
                    if DEBUG:
                        print("强度", data_target_strength_dec)

                    data_target_confidencerate_hex = data[start + 24 + i * Parameter_len:start + 28 + i * Parameter_len]
                    data_target_confidencerate_dec = struct.unpack('f', bytes(data_target_confidencerate_hex))[0]
                    data_target_confidencerate_dec = format(data_target_confidencerate_dec, '.3f')
                    if DEBUG:
                        print("置信度", data_target_confidencerate_dec)

                    # 雷达坐标
                    data_target_Xdistance_hex = data[start + 28 + i * Parameter_len:start + 32 + i * Parameter_len]
                    data_target_Xdistance_dec = struct.unpack('f', bytes(data_target_Xdistance_hex))[0]  # 大端字节
                    data_target_Xdistance_dec = format(data_target_Xdistance_dec, '.3f')
                    if DEBUG:
                        print("X方向距离", data_target_Xdistance_dec)

                    data_target_Ydistance_hex = data[start + 32 + i * Parameter_len:start + 36 + i * Parameter_len]
                    data_target_Ydistance_dec = struct.unpack('f', bytes(data_target_Ydistance_hex))[0]  # 大端字节
                    data_target_Ydistance_dec = format(data_target_Ydistance_dec, '.3f')
                    if DEBUG:
                        print("Y方向距离", data_target_Ydistance_dec)

                    object_point = (float(data_target_Xdistance_dec), float(data_target_Ydistance_dec))
                    object_list.append(object_point)
                    # print("画图-object_list", object_list)  #画图-object_list [('0.430', '10.013')]

                    # 空间坐标X
                    space_target_X_hex = data[start+36 + i * Parameter_len:start+40 + i * Parameter_len]
                    #print("space_target_X_hex:",space_target_X_hex)
                    space_target_X_dec = struct.unpack('f', bytes(space_target_X_hex))[0]
                    space_target_X_dec = format(space_target_X_dec, '.3f')

                    # 空间坐标Y
                    space_target_Y_hex = data[start+40 + i * Parameter_len:start+44 + i * Parameter_len]
                    space_target_Y_dec = struct.unpack('f', bytes(space_target_Y_hex))[0]
                    space_target_Y_dec = format(space_target_Y_dec, '.3f')

                    # 空间坐标Z 雷达角度与图像像素之比
                    space_target_Z_hex = data[start+44 + i * Parameter_len:start+48 + i * Parameter_len]
                    space_target_Z_dec = struct.unpack('f', bytes(space_target_Z_hex))[0]
                    space_target_Z_dec = format(space_target_Z_dec, '.3f')

                    # 后4-5为雷达坐标 后三位为相机坐标
                    data_number_radar_dec = data_number_radar_dec + str(data_time_year_dec) + ',' \
                                            + str(data_time_month_dec) + ',' \
                                            + str(data_time_day_dec) + ',' \
                                            + str(data_time_hour_dec) + ',' \
                                            + str(data_time_minute_dec) + ',' \
                                            + str(data_time_second_dec) + ',' \
                                            + str(data_time_millisecond_dec) + ',' \
                                            + str(data_target_state_dec) + ',' \
                                            + str(data_target_strength_dec) + ',' \
                                            + str(data_target_confidencerate_dec) + ',' \
                                            + str(data_target_Xdistance_dec) + ',' \
                                            + str(data_target_Ydistance_dec) + ',' \
                                            + str(space_target_X_dec) + ',' \
                                            + str(space_target_Y_dec) + ',' \
                                            + str(space_target_Z_dec) + ';' \

                start = start + 9 + data_number_dec * Parameter_len + 1 + 1

                radar_data_file = open("./data/Radar/Radar-object.txt", 'a+')
                radar_data_file.write(data_number_radar_dec + "\n")
                radar_data_file.close()

    return object_list, data_number_radar_dec


# 图像数据解析
def dataImageAnalysis(radar_iamge_hex):
    if (radar_iamge_hex[0:5] == b'\xa5\x5a\xb5\x10\x20'):
        radar_iamge_length_hex = radar_iamge_hex[5:9]
        radar_iamge_length_dec = struct.unpack('i', bytes(radar_iamge_length_hex))[0]  #
        if DEBUG:
            print("目标参数长度", radar_iamge_length_dec)

        radar_iamge_time_year_hex = radar_iamge_hex[9:11]
        radar_iamge_time_year_dec = struct.unpack('h', bytes(radar_iamge_time_year_hex))[0]  #
        if DEBUG:
            print('年', radar_iamge_time_year_dec)

        radar_iamge_time_month_hex = radar_iamge_hex[11:12]
        radar_iamge_time_month_dec = struct.unpack('b', bytes(radar_iamge_time_month_hex))[0]  #
        if DEBUG:
            print('月', radar_iamge_time_month_dec)

        radar_iamge_time_day_hex = radar_iamge_hex[12:13]
        radar_iamge_time_day_dec = struct.unpack('b', bytes(radar_iamge_time_day_hex))[0]  #
        if DEBUG:
            print('日', radar_iamge_time_day_dec)

        # 时
        radar_iamge_time_hour_hex = radar_iamge_hex[13:14]
        radar_iamge_time_hour_dec = struct.unpack('b', bytes(radar_iamge_time_hour_hex))[0]  #

        # 分
        radar_iamge_time_minute_hex = radar_iamge_hex[14:15]
        radar_iamge_time_minute_dec = struct.unpack('b', bytes(radar_iamge_time_minute_hex))[0]  #

        # 秒
        radar_iamge_time_second_hex = radar_iamge_hex[15:16]
        radar_iamge_time_second_dec = struct.unpack('b', bytes(radar_iamge_time_second_hex))[0]  #

        # 毫秒
        radar_iamge_time_millisecond_hex = radar_iamge_hex[16:18]
        radar_iamge_time_millisecond_dec = struct.unpack('h', bytes(radar_iamge_time_millisecond_hex))[0]

        # 图片格式
        radar_iamge_format_hex = radar_iamge_hex[18:19]
        radar_iamge_format = dict_image_format[str(radar_iamge_format_hex.hex())]
        # 19:23 23:27

        data_wall_row_hex = radar_iamge_hex[19:23]
        data_wall_row_dec = struct.unpack('I', bytes(data_wall_row_hex))[0]
        if DEBUG:
            print("宽", data_wall_row_dec)

        data_wall_column_hex = radar_iamge_hex[23:27]
        data_wall_column_dec = struct.unpack('I', bytes(data_wall_column_hex))[0]
        if DEBUG:
            print("高", data_wall_column_dec)

        # 图像像素数据
        radar_iamge_hex = radar_iamge_hex[27:len(radar_iamge_hex) - 1]

        imgagesName = str(radar_iamge_time_year_dec) + "-" + str(radar_iamge_time_month_dec) + "-" + \
                      str(radar_iamge_time_day_dec) + "-" + str(radar_iamge_time_hour_dec) + "-" + \
                      str(radar_iamge_time_minute_dec) + "-" + str(radar_iamge_time_second_dec) + "-" + \
                      str(radar_iamge_time_millisecond_dec) + "." + str(radar_iamge_format)

        # DownloadFile = open(str(radar_iamge_time_year_dec)+"-"+str(radar_iamge_time_month_dec)+"-"+
        #                     str(radar_iamge_time_day_dec)+"-"+str(radar_iamge_time_hour_dec)+"-"+
        #                     str(radar_iamge_time_minute_dec) + "-" + str(radar_iamge_time_second_dec) + "-" +
        #                     str(radar_iamge_time_millisecond_dec)+ "."+str(radar_iamge_format), "wb")
        DownloadFile = open("./data/Radar/" + "Radar-" + imgagesName, "wb")
        DownloadFile.write(radar_iamge_hex)
        DownloadFile.close()

        return imgagesName


class Point:
    def __init__(self, buf):
        self.x, self.y, self.z, self.creditability = struct.unpack("<3id", buf)
        self.x = self.x * 0.01
        self.y = self.y * 0.01
        self.z = self.z * 0.01
    def printIfo(self):
        print("x = " + str(self.x))
        print("y = " + str(self.y))
        print("z = " + str(self.z))
        print("creditability = " + str(self.creditability))


class communication_protocol:
    def __init__(self, buf):
        self.laserLength = struct.unpack("<i", buf[:4])[0]  # int类型，4字节
        self.year, self.month, self.day, self.hour, self.minute, self.second, self.millisecond = struct.unpack(
            "<h5Bh", buf[4:13])  # 9字节
        self.point = []
        for i in range(self.laserLength):
            self.point.append(Point(buf[13 + i * 20:13 + (i + 1) * 20]))

    def printIfo(self):
        print("参数长度 = " + str(self.laserLength))
        print("year = " + str(self.year))
        print("month = " + str(self.month))
        print("day = " + str(self.day))
        print("hour = " + str(self.hour))
        print("minute = " + str(self.minute))
        print("second = " + str(self.second))
        print("millisecond = " + str(self.millisecond))
        for i in range(5):
            self.point[i].printIfo()


# 雷达插值预测相关函数
class RadarSyncData:
    # 传感器数据按时间序列排列，在传感器数据中为同步的时间点找到合适的时间位置
    # unregistered_data最少需要有5个参数，time,data,x,y,z
    # 传入的unregistered_data队列最多有5帧，要考虑到初始阶段不足5帧的情况
    target_type: str
    strength: str
    confidence: str
    sync_data: str
    single_data: str
    complete_data: str
    sum_target_data: str
    synced_data_x: float
    synced_data_y: float
    vx_order: int
    error_vx: float
    vy_order: int
    error_vy: float

    def __init__(self, unregistered_data, sync_time):
        self.multi_unregistered_data = unregistered_data  # multi_unregistered_data输入队列，多目标数据
        self.unregistered_data = deque(maxlen=5)  # unregistered_data为单目标运算时的中间队列
        self.sync_time = sync_time  # 同步时间

    def sync_predict(self):
        # 判断队列 有1帧，直接复制第一帧的数据；有2帧，直接通过上一帧间的速度来预测；有3帧，利用3帧来预测；有4帧，利用4帧来预测；普通情况为5帧，利用前5帧来预测位置
        unregistered_time = []
        unregistered_x = []
        unregistered_y = []
        self.target_type = str(self.unregistered_data.__getitem__(-1).target_type)
        self.strength = str(self.unregistered_data.__getitem__(-1).strength)
        self.confidence = str(self.unregistered_data.__getitem__(-1).confidence)
        if len(self.unregistered_data) == 1:
            # 直接复制第一帧的数据，直到第二帧来到
            self.sync_data = str(self.unregistered_data.__getitem__(0).data)
        elif 1 < len(self.unregistered_data) < 5:
            # 有1帧以上数据，利用前n帧数据做预测
            for i in range(len(self.unregistered_data)):
                unregistered_time.append(float(self.unregistered_data.__getitem__(i).time))
            for i in range(len(self.unregistered_data)):
                unregistered_x.append(float(self.unregistered_data.__getitem__(i).x))
            for i in range(len(self.unregistered_data)):
                unregistered_y.append(float(self.unregistered_data.__getitem__(i).y))
            function_x = np.polyfit(unregistered_time, unregistered_x, 0)
            function_y = np.polyfit(unregistered_time, unregistered_y, 0)
            synced_x = np.polyval(function_x, float(self.sync_time))
            synced_y = np.polyval(function_y, float(self.sync_time))
            synced_data_x = format(synced_x, '.3f')
            synced_data_y = format(synced_y, '.3f')
            self.sync_data = str(synced_data_x + ',' + synced_data_y)
        elif len(self.unregistered_data) == 5:
            # 通常情况下有5帧数据，利用前5帧数据做预测
            unregistered_v_time = []
            unregistered_v_x = []
            unregistered_v_y = []
            for i in range(len(self.unregistered_data) - 1):
                unregistered_time.append(
                    float(self.unregistered_data.__getitem__(i).time))
            for i in range(len(self.unregistered_data) - 1):
                unregistered_x.append(float(self.unregistered_data.__getitem__(i).x))
            for i in range(len(self.unregistered_data) - 1):
                unregistered_y.append(float(self.unregistered_data.__getitem__(i).y))
            for i in range(1, len(self.unregistered_data) - 1):
                unregistered_v_time.append((float(self.unregistered_data.__getitem__(i - 1).time)
                                            + float(self.unregistered_data.__getitem__(i).time)) / 2)  # 中间时刻点
            for i in range(1, len(self.unregistered_data) - 1):
                time_interval = float(self.unregistered_data.__getitem__(i).time) \
                                - float(self.unregistered_data.__getitem__(i - 1).time)
                unregistered_v_x.append((float(self.unregistered_data.__getitem__(i).x)
                                         - float(self.unregistered_data.__getitem__(i - 1).x)) / time_interval)
            for i in range(1, len(self.unregistered_data) - 1):
                time_interval = float(self.unregistered_data.__getitem__(i).time) \
                                - float(self.unregistered_data.__getitem__(i - 1).time)
                unregistered_v_y.append((float(self.unregistered_data.__getitem__(i).y)
                                         - float(self.unregistered_data.__getitem__(i - 1).y)) / time_interval)
            # 利用位置进行多项式预测
            function_x1 = np.polyfit(unregistered_time, unregistered_x, 0)
            function_x2 = np.polyfit(unregistered_time, unregistered_x, 1)
            fifth_time = float(self.unregistered_data.__getitem__(-1).time)
            fifth_x1 = np.polyval(function_x1, fifth_time)
            fifth_x2 = np.polyval(function_x2, fifth_time)
            error_x1 = abs(fifth_x1 - float(self.unregistered_data.__getitem__(-1).x))
            error_x2 = abs(fifth_x2 - float(self.unregistered_data.__getitem__(-1).x))

            function_y1 = np.polyfit(unregistered_time, unregistered_y, 0)
            function_y2 = np.polyfit(unregistered_time, unregistered_y, 1)
            fifth_y1 = np.polyval(function_y1, fifth_time)
            fifth_y2 = np.polyval(function_y2, fifth_time)
            error_y1 = abs(fifth_y1 - float(self.unregistered_data.__getitem__(-1).y))
            error_y2 = abs(fifth_y2 - float(self.unregistered_data.__getitem__(-1).y))

            # 利用速度进行多项式预测
            function_v_x1 = np.polyfit(unregistered_v_time, unregistered_v_x, 0)
            function_v_x2 = np.polyfit(unregistered_v_time, unregistered_v_x, 1)
            fifth_v_time = (float(self.unregistered_data.__getitem__(-2).time)
                            + float(self.unregistered_data.__getitem__(-1).time)) / 2
            last_time_interval = float(self.unregistered_data.__getitem__(-1).time) - float(
                self.unregistered_data.__getitem__(-2).time)
            fifth_vx_1 = np.polyval(function_v_x1, fifth_v_time)
            fifth_vx_2 = np.polyval(function_v_x2, fifth_v_time)
            fifth_truth_vx = (float(self.unregistered_data.__getitem__(-1).x) - float(
                self.unregistered_data.__getitem__(-2).x)) / last_time_interval
            error_v_x1 = abs(fifth_vx_1 - fifth_truth_vx)
            error_v_x2 = abs(fifth_vx_2 - fifth_truth_vx)
            error_min_vx = min(error_v_x1, error_v_x2)
            if error_min_vx == error_v_x1:
                self.vx_order = 0
                predict_fifth_x = float(
                    self.unregistered_data.__getitem__(-2).x) + fifth_vx_1 * last_time_interval
                self.error_vx = abs(predict_fifth_x - float(self.unregistered_data.__getitem__(-1).x))
            elif error_min_vx == error_v_x2:
                self.vx_order = 1
                predict_fifth_x = float(
                    self.unregistered_data.__getitem__(-2).x) + fifth_vx_2 * last_time_interval
                self.error_vx = abs(predict_fifth_x - float(self.unregistered_data.__getitem__(-1).x))

            function_v_y1 = np.polyfit(unregistered_v_time, unregistered_v_y, 0)
            function_v_y2 = np.polyfit(unregistered_v_time, unregistered_v_y, 1)
            fifth_vy_1 = np.polyval(function_v_y1, fifth_v_time)
            fifth_vy_2 = np.polyval(function_v_y2, fifth_v_time)
            fifth_truth_vy = (float(self.unregistered_data.__getitem__(-1).y) - float(
                self.unregistered_data.__getitem__(-2).y)) / last_time_interval
            error_v_y1 = abs(fifth_vy_1 - fifth_truth_vy)
            error_v_y2 = abs(fifth_vy_2 - fifth_truth_vy)
            error_min_vy = min(error_v_y1, error_v_y2)
            if error_min_vy == error_v_y1:
                self.vy_order = 0
                predict_fifth_y = float(
                    self.unregistered_data.__getitem__(-2).y) + fifth_vy_1 * last_time_interval
                self.error_vy = abs(predict_fifth_y - float(self.unregistered_data.__getitem__(-1).y))
            elif error_min_vy == error_v_y2:
                self.vy_order = 1
                predict_fifth_y = float(
                    self.unregistered_data.__getitem__(-2).y) + fifth_vy_2 * last_time_interval
                self.error_vy = abs(predict_fifth_y - float(self.unregistered_data.__getitem__(-1).y))

            # 将五个点都放入，准备预测同步点坐标
            unregistered_time.append(float(self.unregistered_data.__getitem__(-1).time))
            unregistered_x.append(float(self.unregistered_data.__getitem__(-1).x))
            unregistered_y.append(float(self.unregistered_data.__getitem__(-1).y))
            unregistered_v_time.append((float(self.unregistered_data.__getitem__(-2).time) + float(
                self.unregistered_data.__getitem__(-1).time)) / 2)
            unregistered_v_x.append((float(self.unregistered_data.__getitem__(-1).x) - float(
                self.unregistered_data.__getitem__(-2).x)) / last_time_interval)
            unregistered_v_y.append((float(self.unregistered_data.__getitem__(-1).y) - float(
                self.unregistered_data.__getitem__(-2).y)) / last_time_interval)

            error_min_x = min(error_x1, error_x2, self.error_vx)
            if error_min_x == error_x1:
                function_x = np.polyfit(unregistered_time, unregistered_x, 0)
                self.synced_data_x = float(np.polyval(function_x, float(self.sync_time)))
            elif error_min_x == error_x2:
                function_x = np.polyfit(unregistered_time, unregistered_x, 1)
                self.synced_data_x = float(np.polyval(function_x, float(self.sync_time)))
            elif error_min_x == self.error_vx:
                function_v_x = np.polyfit(unregistered_v_time, unregistered_v_x, self.vx_order)
                sync_v_time = (float(self.unregistered_data.__getitem__(-1).time) + float(self.sync_time)) / 2
                sync_velocity_x = np.polyval(function_v_x, sync_v_time)
                sync_time_interval = float(self.sync_time) - float(self.unregistered_data.__getitem__(-1).time)
                self.synced_data_x = float(
                    self.unregistered_data.__getitem__(-1).x) + sync_velocity_x * sync_time_interval

            error_min_y = min(error_y1, error_y2, self.error_vy)
            if error_min_y == error_y1:
                function_y = np.polyfit(unregistered_time, unregistered_y, 0)
                self.synced_data_y = float(np.polyval(function_y, float(self.sync_time)))
            elif error_min_y == error_y2:
                function_y = np.polyfit(unregistered_time, unregistered_y, 1)
                self.synced_data_y = float(np.polyval(function_y, float(self.sync_time)))
            elif error_min_y == self.error_vy:
                function_v_y = np.polyfit(unregistered_v_time, unregistered_v_y, self.vy_order)
                sync_v_time = (float(self.unregistered_data.__getitem__(-1).time) + float(self.sync_time)) / 2
                sync_velocity_y = np.polyval(function_v_y, sync_v_time)
                sync_time_interval = float(self.sync_time) - float(self.unregistered_data.__getitem__(-1).time)
                self.synced_data_y = float(
                    self.unregistered_data.__getitem__(-1).y) + sync_velocity_y * sync_time_interval

            self.sync_data = str(format(self.synced_data_x, '.3f') + ',' + format(self.synced_data_y, '.3f'))
        self.complete_data = str(self.target_type + ',' + self.strength + ',' + self.confidence + ',' + self.sync_data)
        return self.complete_data

    def sync_interpolation(self):
        # 判断队列 有1帧，直接复制第一帧的数据；有2~5帧，直接通过多帧之间插值得到；普通情况为5帧，通过位置和速度的插值计算得到
        self.target_type = str(self.unregistered_data.__getitem__(-1).target_type)
        self.strength = str(self.unregistered_data.__getitem__(-1).strength)
        self.confidence = str(self.unregistered_data.__getitem__(-1).confidence)
        if len(self.unregistered_data) == 1:
            # 直接复制第一帧的数据，直到雷达的下一帧来临
            self.sync_data = str(self.unregistered_data.__getitem__(0).data)

        elif 2 <= len(self.unregistered_data) <= 5:
            # 有大于1帧数据，直接利用这几帧插值计算中间位置
            unregistered_time = []
            unregistered_x = []
            unregistered_y = []
            for i in range(len(self.unregistered_data)):
                unregistered_time.append(float(self.unregistered_data.__getitem__(i).time))
            for i in range(len(self.unregistered_data)):
                unregistered_x.append(float(self.unregistered_data.__getitem__(i).x))
            for i in range(len(self.unregistered_data)):
                unregistered_y.append(float(self.unregistered_data.__getitem__(i).y))
            self.synced_data_x = float(np.interp(float(self.sync_time), unregistered_time, unregistered_x))
            self.synced_data_y = float(np.interp(float(self.sync_time), unregistered_time, unregistered_y))
            self.sync_data = str(format(self.synced_data_x, '.3f') + ',' + format(self.synced_data_y, '.3f'))
        self.complete_data = str(self.target_type + ',' + self.strength + ',' + self.confidence + ',' + self.sync_data)
        return self.complete_data

    def one_target_registration(self):
        if (float(self.sync_time) - float(self.unregistered_data[-1].time)) >= 0.5:  # 若时间差过大，直接返回空值
            self.single_data = 'None'
        elif (float(self.sync_time) - float(self.unregistered_data[-1].time)) < 0.5:  # 0.5s的时间差根据实际情况改变
            # 同步时间在队列最新数据之后，需要预测手段
            if float(self.unregistered_data[-1].time) <= float(self.sync_time):
                self.single_data = self.sync_predict()
            # 同步时间在队列最新数据之前，需要进行插值方法
            elif float(self.unregistered_data[-1].time) > float(self.sync_time):
                self.single_data = self.sync_interpolation()
        return self.single_data

    def radar_registration(self):
        self.sum_target_data = ''  # 初始化
        for target_id in self.multi_unregistered_data[-1].keys():  # 通过最新帧的目标ID去寻找前几帧的数据
            self.unregistered_data.clear()  # 每一个目标都需要全新的队列
            for i in range(0, len(self.multi_unregistered_data)):  # 读取前几帧的数据，查找有无相同ID的目标
                data = self.multi_unregistered_data[i].get(target_id)  # 提取相应目标ID的数据
                if data is not None:
                    self.unregistered_data.append(data)  # 如果有相同的目标则添加进未配准队列中
            self.sum_target_data += self.one_target_registration() + ';'  # 将所有配准好的目标通过字符串组合到一起
        return self.sum_target_data


# 声源插值预测相关函数
class SoundSyncData:
    # 传感器数据按时间序列排列，在传感器数据中为同步的时间点找到合适的时间位置
    # unregistered_data最少需要有5个参数，time,data,x,y,z
    # 传入的unregistered_data队列最多有5帧，要考虑到初始阶段不足5帧的情况
    confidence: str
    sync_data: str
    complete_data: str
    single_data: str
    complete_data: str
    sum_target_data: str
    synced_data_x: float
    synced_data_y: float
    synced_data_z: float
    vx_order: int
    error_vx: float
    vy_order: int
    error_vy: float
    vz_order: int
    error_vz: float

    def __init__(self, unregistered_data, sync_time):
        self.multi_unregistered_data = unregistered_data  # multi_unregistered_data输入队列，多目标数据
        self.unregistered_data = deque(maxlen=8)  # unregistered_data为单目标运算时的中间队列
        self.sync_time = sync_time  # 同步时间

    def sync_predict(self):
        # 判断队列 有1帧，直接复制第一帧的数据；有2帧，利用2帧来预测；有3帧，利用3帧来预测；有4帧，利用4帧来预测；普通情况为5帧，利用前5帧来预测位置
        unregistered_time = []
        unregistered_x = []
        unregistered_y = []
        unregistered_z = []
        self.confidence = str(self.unregistered_data.__getitem__(-1).confidence)
        if len(self.unregistered_data) == 1:
            # 直接复制第一帧的数据，直到第二帧来到
            self.sync_data = str(self.unregistered_data.__getitem__(0).data)
        elif 1 < len(self.unregistered_data) < 8:
            # 有1帧以上数据，利用前n帧数据做预测
            for i in range(len(self.unregistered_data)):
                unregistered_time.append(float(self.unregistered_data.__getitem__(i).time))
            for i in range(len(self.unregistered_data)):
                unregistered_x.append(float(self.unregistered_data.__getitem__(i).x))
            for i in range(len(self.unregistered_data)):
                unregistered_y.append(float(self.unregistered_data.__getitem__(i).y))
            for i in range(len(self.unregistered_data)):
                unregistered_z.append(float(self.unregistered_data.__getitem__(i).z))
            function_x = np.polyfit(unregistered_time, unregistered_x, 0)
            function_y = np.polyfit(unregistered_time, unregistered_y, 0)
            function_z = np.polyfit(unregistered_time, unregistered_z, 0)
            synced_data_x = np.polyval(function_x, float(self.sync_time))
            synced_data_y = np.polyval(function_y, float(self.sync_time))
            synced_data_z = np.polyval(function_z, float(self.sync_time))
            self.sync_data = str(str(format(synced_data_x, '.3f')) + ',' + str(format(synced_data_y, '.3f'))
                                 + ',' + str(format(synced_data_z, '.3f')))
        elif len(self.unregistered_data) == 8:
            # 通常情况下有5帧数据，利用前5帧数据做预测
            unregistered_v_time = []
            unregistered_v_x = []
            unregistered_v_y = []
            unregistered_v_z = []
            for i in range(len(self.unregistered_data) - 1):
                unregistered_time.append(
                    float(self.unregistered_data.__getitem__(i).time))
            for i in range(len(self.unregistered_data) - 1):
                unregistered_x.append(float(self.unregistered_data.__getitem__(i).x))
            for i in range(len(self.unregistered_data) - 1):
                unregistered_y.append(float(self.unregistered_data.__getitem__(i).y))
            for i in range(len(self.unregistered_data) - 1):
                unregistered_z.append(float(self.unregistered_data.__getitem__(i).z))
            for i in range(1, len(self.unregistered_data) - 1):
                unregistered_v_time.append((float(self.unregistered_data.__getitem__(i - 1).time)
                                            + float(self.unregistered_data.__getitem__(i).time)) / 2)  # 中间时刻点
            for i in range(1, len(self.unregistered_data) - 1):
                time_interval = float(self.unregistered_data.__getitem__(i).time) \
                                - float(self.unregistered_data.__getitem__(i - 1).time)
                unregistered_v_x.append((float(self.unregistered_data.__getitem__(i).x)
                                         - float(self.unregistered_data.__getitem__(i - 1).x)) / time_interval)
            for i in range(1, len(self.unregistered_data) - 1):
                time_interval = float(self.unregistered_data.__getitem__(i).time) \
                                - float(self.unregistered_data.__getitem__(i - 1).time)
                unregistered_v_y.append((float(self.unregistered_data.__getitem__(i).y)
                                         - float(self.unregistered_data.__getitem__(i - 1).y)) / time_interval)
            for i in range(1, len(self.unregistered_data) - 1):
                time_interval = float(self.unregistered_data.__getitem__(i).time) \
                                - float(self.unregistered_data.__getitem__(i - 1).time)
                unregistered_v_z.append((float(self.unregistered_data.__getitem__(i).z)
                                         - float(self.unregistered_data.__getitem__(i - 1).z)) / time_interval)
            # 利用位置进行多项式预测
            function_x1 = np.polyfit(unregistered_time, unregistered_x, 0)
            function_x2 = np.polyfit(unregistered_time, unregistered_x, 1)
            fifth_time = float(self.unregistered_data.__getitem__(-1).time)
            fifth_x1 = np.polyval(function_x1, fifth_time)
            fifth_x2 = np.polyval(function_x2, fifth_time)
            error_x1 = abs(fifth_x1 - float(self.unregistered_data.__getitem__(-1).x))
            error_x2 = abs(fifth_x2 - float(self.unregistered_data.__getitem__(-1).x))

            function_y1 = np.polyfit(unregistered_time, unregistered_y, 0)
            function_y2 = np.polyfit(unregistered_time, unregistered_y, 1)
            fifth_y1 = np.polyval(function_y1, fifth_time)
            fifth_y2 = np.polyval(function_y2, fifth_time)
            error_y1 = abs(fifth_y1 - float(self.unregistered_data.__getitem__(-1).y))
            error_y2 = abs(fifth_y2 - float(self.unregistered_data.__getitem__(-1).y))

            function_z1 = np.polyfit(unregistered_time, unregistered_z, 0)
            function_z2 = np.polyfit(unregistered_time, unregistered_z, 1)
            fifth_z1 = np.polyval(function_z1, fifth_time)
            fifth_z2 = np.polyval(function_z2, fifth_time)
            error_z1 = abs(fifth_z1 - float(self.unregistered_data.__getitem__(-1).z))
            error_z2 = abs(fifth_z2 - float(self.unregistered_data.__getitem__(-1).z))

            # 利用速度进行多项式预测
            function_v_x1 = np.polyfit(unregistered_v_time, unregistered_v_x, 0)
            function_v_x2 = np.polyfit(unregistered_v_time, unregistered_v_x, 1)
            fifth_v_time = (float(self.unregistered_data.__getitem__(-2).time)
                            + float(self.unregistered_data.__getitem__(-1).time)) / 2
            last_time_interval = float(self.unregistered_data.__getitem__(-1).time) - float(
                self.unregistered_data.__getitem__(-2).time)
            fifth_vx_1 = np.polyval(function_v_x1, fifth_v_time)
            fifth_vx_2 = np.polyval(function_v_x2, fifth_v_time)
            fifth_truth_vx = (float(self.unregistered_data.__getitem__(-1).x) - float(
                self.unregistered_data.__getitem__(-2).x)) / last_time_interval
            error_v_x1 = abs(fifth_vx_1 - fifth_truth_vx)
            error_v_x2 = abs(fifth_vx_2 - fifth_truth_vx)
            error_min_vx = min(error_v_x1, error_v_x2)
            if error_min_vx == error_v_x1:
                self.vx_order = 0
                predict_fifth_x = float(
                    self.unregistered_data.__getitem__(-2).x) + fifth_vx_1 * last_time_interval
                self.error_vx = abs(predict_fifth_x - float(self.unregistered_data.__getitem__(-1).x))
            elif error_min_vx == error_v_x2:
                self.vx_order = 1
                predict_fifth_x = float(
                    self.unregistered_data.__getitem__(-2).x) + fifth_vx_2 * last_time_interval
                self.error_vx = abs(predict_fifth_x - float(self.unregistered_data.__getitem__(-1).x))

            function_v_y1 = np.polyfit(unregistered_v_time, unregistered_v_y, 0)
            function_v_y2 = np.polyfit(unregistered_v_time, unregistered_v_y, 1)
            fifth_vy_1 = np.polyval(function_v_y1, fifth_v_time)
            fifth_vy_2 = np.polyval(function_v_y2, fifth_v_time)
            fifth_truth_vy = (float(self.unregistered_data.__getitem__(-1).y) - float(
                self.unregistered_data.__getitem__(-2).y)) / last_time_interval
            error_v_y1 = abs(fifth_vy_1 - fifth_truth_vy)
            error_v_y2 = abs(fifth_vy_2 - fifth_truth_vy)
            error_min_vy = min(error_v_y1, error_v_y2)
            if error_min_vy == error_v_y1:
                self.vy_order = 0
                predict_fifth_y = float(
                    self.unregistered_data.__getitem__(-2).y) + fifth_vy_1 * last_time_interval
                self.error_vy = abs(predict_fifth_y - float(self.unregistered_data.__getitem__(-1).y))
            elif error_min_vy == error_v_y2:
                self.vy_order = 1
                predict_fifth_y = float(
                    self.unregistered_data.__getitem__(-2).y) + fifth_vy_2 * last_time_interval
                self.error_vy = abs(predict_fifth_y - float(self.unregistered_data.__getitem__(-1).y))

            function_v_z1 = np.polyfit(unregistered_v_time, unregistered_v_z, 0)
            function_v_z2 = np.polyfit(unregistered_v_time, unregistered_v_z, 1)
            fifth_vz_1 = np.polyval(function_v_z1, fifth_v_time)
            fifth_vz_2 = np.polyval(function_v_z2, fifth_v_time)
            fifth_truth_vz = (float(self.unregistered_data.__getitem__(-1).z) - float(
                self.unregistered_data.__getitem__(-2).z)) / last_time_interval
            error_v_z1 = abs(fifth_vz_1 - fifth_truth_vz)
            error_v_z2 = abs(fifth_vz_2 - fifth_truth_vz)
            error_min_vz = min(error_v_z1, error_v_z2)
            if error_min_vz == error_v_z1:
                self.vz_order = 0
                predict_fifth_z = float(
                    self.unregistered_data.__getitem__(-2).z) + fifth_vz_1 * last_time_interval
                self.error_vz = abs(predict_fifth_z - float(self.unregistered_data.__getitem__(-1).z))
            elif error_min_vz == error_v_z2:
                self.vz_order = 1
                predict_fifth_z = float(
                    self.unregistered_data.__getitem__(-2).z) + fifth_vz_2 * last_time_interval
                self.error_vz = abs(predict_fifth_z - float(self.unregistered_data.__getitem__(-1).z))

            # 将五个点都放入，准备预测同步点坐标
            unregistered_time.append(float(self.unregistered_data.__getitem__(-1).time))
            unregistered_x.append(float(self.unregistered_data.__getitem__(-1).x))
            unregistered_y.append(float(self.unregistered_data.__getitem__(-1).y))
            unregistered_z.append(float(self.unregistered_data.__getitem__(-1).z))
            unregistered_v_time.append((float(self.unregistered_data.__getitem__(-2).time) + float(
                self.unregistered_data.__getitem__(-1).time)) / 2)
            unregistered_v_x.append((float(self.unregistered_data.__getitem__(-1).x) - float(
                self.unregistered_data.__getitem__(-2).x)) / last_time_interval)
            unregistered_v_y.append((float(self.unregistered_data.__getitem__(-1).y) - float(
                self.unregistered_data.__getitem__(-2).y)) / last_time_interval)
            unregistered_v_z.append((float(self.unregistered_data.__getitem__(-1).z) - float(
                self.unregistered_data.__getitem__(-2).z)) / last_time_interval)

            error_min_x = min(error_x1, error_x2, self.error_vx)
            if error_min_x == error_x1:
                function_x = np.polyfit(unregistered_time, unregistered_x, 0)
                self.synced_data_x = float(np.polyval(function_x, float(self.sync_time)))
            elif error_min_x == error_x2:
                function_x = np.polyfit(unregistered_time, unregistered_x, 1)
                self.synced_data_x = float(np.polyval(function_x, float(self.sync_time)))
            elif error_min_x == self.error_vx:
                function_v_x = np.polyfit(unregistered_v_time, unregistered_v_x, self.vx_order)
                sync_v_time = (float(self.unregistered_data.__getitem__(-1).time) + float(self.sync_time)) / 2
                sync_velocity_x = np.polyval(function_v_x, sync_v_time)
                sync_time_interval = float(self.sync_time) - float(self.unregistered_data.__getitem__(-1).time)
                position_diff_x = sync_velocity_x * sync_time_interval
                self.synced_data_x = float(self.unregistered_data.__getitem__(-1).x) + position_diff_x

            error_min_y = min(error_y1, error_y2, self.error_vy)
            if error_min_y == error_y1:
                function_y = np.polyfit(unregistered_time, unregistered_y, 0)
                self.synced_data_y = float(np.polyval(function_y, float(self.sync_time)))
            elif error_min_y == error_y2:
                function_y = np.polyfit(unregistered_time, unregistered_y, 1)
                self.synced_data_y = float(np.polyval(function_y, float(self.sync_time)))
            elif error_min_y == self.error_vy:
                function_v_y = np.polyfit(unregistered_v_time, unregistered_v_y, self.vy_order)
                sync_v_time = (float(self.unregistered_data.__getitem__(-1).time) + float(self.sync_time)) / 2
                sync_velocity_y = np.polyval(function_v_y, sync_v_time)
                sync_time_interval = float(self.sync_time) - float(self.unregistered_data.__getitem__(-1).time)
                position_diff_y = sync_velocity_y * sync_time_interval
                self.synced_data_y = float(self.unregistered_data.__getitem__(-1).y) + position_diff_y

            error_min_z = min(error_z1, error_z2, self.error_vz)
            if error_min_z == error_z1:
                function_z = np.polyfit(unregistered_time, unregistered_z, 0)
                self.synced_data_z = float(np.polyval(function_z, float(self.sync_time)))
            elif error_min_z == error_z2:
                function_z = np.polyfit(unregistered_time, unregistered_z, 1)
                self.synced_data_z = float(np.polyval(function_z, float(self.sync_time)))
            elif error_min_z == self.error_vz:
                function_v_z = np.polyfit(unregistered_v_time, unregistered_v_z, self.vz_order)
                sync_v_time = (float(self.unregistered_data.__getitem__(-1).time) + float(self.sync_time)) / 2
                sync_velocity_z = np.polyval(function_v_z, sync_v_time)
                sync_time_interval = float(self.sync_time) - float(self.unregistered_data.__getitem__(-1).time)
                position_diff_z = sync_velocity_z * sync_time_interval
                self.synced_data_z = float(self.unregistered_data.__getitem__(-1).z) + position_diff_z

            self.sync_data = str(format(self.synced_data_x, '.3f') + ',' + format(self.synced_data_y, '.3f')
                                 + ',' + format(self.synced_data_z, '.3f'))
        self.complete_data = str(self.confidence + ',' + self.sync_data)
        return self.complete_data

    def sync_interpolation(self):
        # 判断队列 有1帧，直接复制第一帧的数据；有2~5帧，直接通过多帧之间插值得到；普通情况为5帧，通过位置和速度的插值计算得到
        self.confidence = str(self.unregistered_data.__getitem__(-1).confidence)
        if len(self.unregistered_data) == 1:
            # 直接复制第一帧的数据，直到雷达的下一帧来临
            self.sync_data = str(self.unregistered_data.__getitem__(0).data)

        elif 2 <= len(self.unregistered_data) <= 8:
            # 有大于1帧数据，直接利用这几帧插值计算中间位置
            unregistered_time = []
            unregistered_x = []
            unregistered_y = []
            unregistered_z = []
            for i in range(len(self.unregistered_data)):
                unregistered_time.append(float(self.unregistered_data.__getitem__(i).time))
            for i in range(len(self.unregistered_data)):
                unregistered_x.append(float(self.unregistered_data.__getitem__(i).x))
            for i in range(len(self.unregistered_data)):
                unregistered_y.append(float(self.unregistered_data.__getitem__(i).y))
            for i in range(len(self.unregistered_data)):
                unregistered_z.append(float(self.unregistered_data.__getitem__(i).z))
            synced_data_x = np.interp(float(self.sync_time), unregistered_time, unregistered_x)
            synced_data_y = np.interp(float(self.sync_time), unregistered_time, unregistered_y)
            synced_data_z = np.interp(float(self.sync_time), unregistered_time, unregistered_z)
            self.sync_data = str(format(synced_data_x, '.3f') + ',' + format(synced_data_y, '.3f')
                                 + ',' + format(synced_data_z, '.3f'))
        self.complete_data = str(self.confidence + ',' + self.sync_data)
        return self.complete_data

    def one_target_registration(self):
        if (float(self.sync_time) - float(self.unregistered_data[-1].time)) >= 2.1:  # 若时间差过大，直接返回空值
            self.single_data = 'None'
        elif (float(self.sync_time) - float(self.unregistered_data[-1].time)) < 2.1:  # 1.0s的时间差根据实际情况改变
            # 同步时间在队列最新数据之后，需要预测手段
            if float(self.unregistered_data[-1].time) <= float(self.sync_time):
                self.single_data = self.sync_predict()
            # 同步时间在队列最新数据之前，需要进行插值方法
            elif float(self.unregistered_data[-1].time) > float(self.sync_time):
                self.single_data = self.sync_interpolation()
        return self.single_data

    def sound_registration(self):
        self.sum_target_data = ''  # 初始化
        for target_id in self.multi_unregistered_data[-1].keys():  # 通过最新帧的目标ID去寻找前几帧的数据
            self.unregistered_data.clear()  # 每一个目标都需要全新的队列
            for i in range(0, len(self.multi_unregistered_data)):  # 读取前几帧的数据，查找有无相同ID的目标
                data = self.multi_unregistered_data[i].get(target_id)  # 提取相应目标ID的数据
                if data is not None:
                    self.unregistered_data.append(data)  # 如果有相同的目标则添加进未配准队列中
            self.sum_target_data += self.one_target_registration() + ';'  # 将所有配准好的目标通过字符串组合到一起
        return self.sum_target_data


# 雷达数据提取函数
class RadarPreprocessing:
    def __init__(self, come_data):
        # 先读取数据，将数据读出来
        # 'number;year,month,day,hour,minute,second,millisecond,target_type,strength,confidence,x,y;
        #         year,month,day,hour,minute,second,millisecond,target_type,strength,confidence,x,y;
        #         year,month,day,hour,minute,second,millisecond,target_type,strength,confidence,x,y;'
        self.split_data = re.split(r';', come_data)
        self.number = int(self.split_data[0])  # number表示目标的数量
        time_data = re.split(r',', self.split_data[1])
        hour = time_data[3]
        minute = time_data[4]
        second = time_data[5]
        millisecond = time_data[6]
        self.time = int(hour) * 3600 + int(minute) * 60 + int(second) + int(millisecond) / 1000

    def target_preprocess(self):
        radar_target_deque.clear()  # 将该队列清零，重新初始化
        for i in range(0, self.number):
            radar_time = self.time
            radar_data = self.split_data[i + 1]
            radar_target = str(radar_time) + ',' + radar_data
            radar_target_deque.append(radar_target)  # 将各目标数据以字符串形式依次放入待处理队列中
        return self.number

    def radar_target_preprocess(self):
        radar_target_precess.clear()  # 将该队列清零，重新初始化
        for i in range(0, self.number):
            radar_time = self.time
            radar_data = self.split_data[i + 1]
            radar_target = str(radar_time) + ',' + radar_data
            radar_target_precess.append(radar_target)  # 将各目标数据以字符串形式依次放入待处理队列中
        return self.number


# 雷达目标提取函数
class RadarTargetPreprocessing:
    def __init__(self, come_data):
        # 处理单目标数据，输入单目标数据内容
        # 'time,year,month,day,hour,minute,second,millisecond,target_type,strength,confidence,x,y;'
        radar_data = re.split(r',', come_data)
        radar_time = float(radar_data[0])
        target_type = radar_data[8]  # 1：运动目标 2：静止目标
        strength = radar_data[9]
        confidence = radar_data[10]
        x = radar_data[11]
        y = radar_data[12]
        self.time = radar_time
        self.target_type = target_type
        self.strength = strength
        self.confidence = confidence
        self.x = x
        self.y = y
        self.data = x + ',' + y
        self.sure_data = radar_data[8] + ',' + radar_data[9] + ',' + radar_data[10] + ',' + radar_data[11] + ',' +\
                         radar_data[12] + ';'
        self.complete_data = radar_data[1] + ',' + radar_data[2] + ',' + radar_data[3] + ',' + radar_data[4] + ',' + \
                             radar_data[5] + ',' + radar_data[6] + ',' + radar_data[7] + ',' + radar_data[8] + ',' + \
                             radar_data[9] + ',' + radar_data[10] + ',' + radar_data[11] + ',' + radar_data[12] + ';'


# 平均雷达目标函数
class AverRadarTargetPreprocessing:
    def __init__(self, come_data, x, y):
        # 处理单目标数据，输入单目标数据内容
        # 'year,month,day,hour,minute,second,millisecond,target_type,strength,confidence,x,y;'
        radar_data = re.split(r',', come_data)
        radar_time = float(radar_data[0])
        target_type = radar_data[8]  # 1：运动目标 2：静止目标
        strength = radar_data[9]
        confidence = radar_data[10]
        self.time = radar_time
        self.target_type = target_type
        self.strength = strength
        self.confidence = confidence
        self.x = str(x)
        self.y = str(y)
        self.data = self.x + ',' + self.y
        self.sure_data = radar_data[7] + ',' + radar_data[8] + ',' + radar_data[9] + ',' + self.x + ',' + self.y + ';'
        self.complete_data = radar_data[0] + ',' + radar_data[1] + ',' + radar_data[2] + ',' + radar_data[3] + ',' + \
                             radar_data[4] + ',' + radar_data[5] + ',' + radar_data[6] + ',' + radar_data[7] + ',' + \
                             radar_data[8] + ',' + radar_data[9] + ',' + self.x + ',' + self.y + ';'


# 雷达目标的数据关联
class RadarDataAssociation:
    number: int  # 目标数量
    pre: int  # 中间变量
    cur: int  # 中间变量

    def __init__(self, come_data):
        self.data = come_data
        self.previous_frame = OrderedDict()  # 上一帧数据的字典
        self.current_frame = OrderedDict()  # 当前帧数据的字典
        self.history_frame = OrderedDict()  # 历史前几帧数据的字典
        self.noise_target_frame = OrderedDict()  # 创建一个新的字典，用来存储疑似噪声目标
        self.association_target = OrderedDict()  # 最终返回的已数据关联的字典
        self.noise_error = 2.0  # 噪声误差阈值，若目标位置误差大于该值，则判断其为噪声或是新的目标
        self.history_target_error = 1.0  # 旧目标误差阈值，若目标误差与历史帧中任何一帧中目标误差小于该值，则判断其为该历史目标

    def data_instantiate(self):
        # 将该帧中的目标数据实例化，并存成字典的格式
        self.number = RadarPreprocessing(self.data).target_preprocess()  # 对雷达数据做预处理
        target_dictionary = OrderedDict()  # 创建一个空的有序字典，用来储存该帧中各目标数据
        for i in range(0, self.number):
            target = RadarTargetPreprocessing(radar_target_deque[i])  # 将雷达数据中所有目标实例化
            target_dictionary['target_' + str(i)] = target  # 各目标按顺序以键为target_0,1,2...存储目标数据值
        return target_dictionary

    def build_association(self):
        # 构建关联矩阵，列为上一帧目标，行为当前帧目标
        row = len(self.previous_frame)  # 关联矩阵的行数——上一帧数据的目标数量
        col = len(self.current_frame)  # 关联矩阵的列数——当前帧数据的目标数量
        association_matrix = np.zeros(shape=(row, col))  # 建立空的关联矩阵
        pre_keys = []  # 建立存储上帧数据字典的键的列表
        for pre_target_id in self.previous_frame.keys():
            pre_keys.append(pre_target_id)  # 将上帧数据的键添加进列表中
        cur_keys = []  # 建立存储当前数据字典的键的列表
        for cur_target_id in self.current_frame.keys():
            cur_keys.append(cur_target_id)  # 将当前数据的键添加进列表中
        for i in range(row):
            pre_target = self.previous_frame.get(pre_keys[i])  # 依次提取上帧的目标数据
            pre_position_x = float(pre_target.x)
            pre_position_y = float(pre_target.y)
            for j in range(col):
                cur_target = self.current_frame.get(cur_keys[j])  # 依次提取当前帧的目标数据
                cur_position_x = float(cur_target.x)
                cur_position_y = float(cur_target.y)
                error_x = abs(pre_position_x - cur_position_x)
                error_y = abs(pre_position_y - cur_position_y)
                error = (error_x ** 2 + error_y ** 2) ** 0.5  # 计算上帧其一目标与当前帧其一目标的位置误差
                association_matrix[i][j] = error  # 将误差放入关联矩阵中
        return association_matrix, cur_keys, pre_keys

    def build_noise_association(self):
        # 构建噪声的关联矩阵，列为上一帧目标，行为当前帧目标
        row = len(self.history_frame)  # 关联矩阵的行数——上一帧数据的目标数量
        col = len(self.noise_target_frame)  # 关联矩阵的列数——当前帧数据的目标数量
        noise_association_matrix = np.zeros(shape=(row, col))  # 建立空的关联矩阵
        his_keys = []  # 建立存储上帧数据字典的键的列表
        for his_target_id in self.history_frame.keys():
            his_keys.append(his_target_id)  # 将上帧数据的键添加进列表中
        no_keys = []  # 建立存储当前数据字典的键的列表
        for no_target_id in self.noise_target_frame.keys():
            no_keys.append(no_target_id)  # 将当前数据的键添加进列表中
        for i in range(row):
            his_target = self.history_frame.get(his_keys[i])  # 依次提取上帧的目标数据
            his_position_x = float(his_target.x)
            his_position_y = float(his_target.y)
            for j in range(col):
                no_target = self.noise_target_frame.get(no_keys[j])  # 依次提取当前帧的目标数据
                no_position_x = float(no_target.x)
                no_position_y = float(no_target.y)
                error_x = abs(his_position_x - no_position_x)
                error_y = abs(his_position_y - no_position_y)
                error = (error_x ** 2 + error_y ** 2) ** 0.5  # 计算上帧其一目标与当前帧其一目标的位置误差
                noise_association_matrix[i][j] = error  # 将误差放入关联矩阵中
        return noise_association_matrix, no_keys, his_keys

    def data_association(self):
        # 初始状态
        if len(unregistered_radar) == 0:
            # 全部为新目标
            self.association_target = self.data_instantiate()  # 首帧，直接实例+字典化，然后返回该帧数据
            sum_radar_target.append(self.number)  # 对目标数量队列初始化，第一帧的目标总数
            return self.association_target
        # 非初始状态
        elif len(unregistered_radar) > 0:
            # 上一帧目标 + 前历史帧目标 + 噪声/新目标
            self.previous_frame = unregistered_radar[-1]  # 先提取上一帧的目标数据
            self.current_frame = self.data_instantiate()  # 对当前帧做实例+字典化
            be_association_matrix, be_cur_keys, pre_keys = self.build_association()
            # 至此，构建好了初始的关联矩阵，列为上一帧目标，行为当前帧目标，下面要进行初始去噪
            min_col = np.min(be_association_matrix, axis=0)  # 找到关联矩阵的每一列中的最小值,生成一个列表
            noise_target_col = []  # 创建储存噪声/新目标目标点的集合，储存其列号
            for i in range(len(min_col)):  # 遍历关联矩阵的每一列
                if min_col[i] > self.noise_error:
                    noise_target_col.append(i)  # 若min的值都大于阈值，则判断为噪声，添加进噪声/新目标列表
            # 检验是否存在以往目标在上一帧中丢失的情况但在前几帧中一直存在的情况，保证精确匹配
            if len(noise_target_col) != 0:
                for m in range(len(noise_target_col)):  # 将所有疑似噪声重新弄一个字典
                    noise_target_id = noise_target_col[m]  # 提取噪声/目标所在的列号
                    noise_target_key = be_cur_keys[noise_target_id]  # 提取噪声/目标的键值
                    self.noise_target_frame[noise_target_key] = self.current_frame[noise_target_key]  # 添加疑似噪声目标进新字典
                for n in range(len(unregistered_radar) - 1):  # 从前几帧的历史帧中依次寻找
                    self.history_frame = unregistered_radar[len(unregistered_radar) - n - 1]  # 从历史帧中获取数据，检验是否存在相似目标
                    if self.noise_target_frame == 0 or n == 4:  # 若所有疑似噪声已经匹配完毕，退出循环
                        break
                    noise_association_matrix, no_keys, his_keys = self.build_noise_association()  # 构建疑似噪声和历史帧的关联矩阵
                    min_target_error = np.min(noise_association_matrix, axis=0)  # 找到该关联矩阵的每一列中的最小值,生成一个列表
                    for i in range(0, len(min_target_error)):  # 遍历关联矩阵的每一列
                        if min_target_error[i] < self.history_target_error:  # 若误差小于阈值
                            old_noise_target_key = no_keys[i]  # 获取该疑似旧目标的疑似噪声的键值
                            no_row, no_col = noise_association_matrix.shape  # 提取关联矩阵的行和列数
                            for j in range(0, no_row):  # 在矩阵的这一列中寻找
                                if noise_association_matrix[j][i] == min_target_error[i]:  # 若找到该最小误差值
                                    new_his_target_id = j  # 找到旧目标的行号
                                    new_his_target_key = his_keys[new_his_target_id]  # 通过其列号找到该旧目标的键值
                                    self.association_target[new_his_target_key] = self.noise_target_frame.pop(
                                        old_noise_target_key)  # 更改目标的键值，并放入最终字典
                                    del self.current_frame[old_noise_target_key]  # 匹配完成，将该目标从当前帧字典中删去
                                    break
            # 前历史帧目标完成
            if len(self.noise_target_frame) > 0:
                for noise_key in self.noise_target_frame.keys():
                    new_number = sum_radar_target[-1] + 1  # 获取最新的添加目标的id值
                    sum_radar_target.append(new_number)  # 将最新的总目标数量更新到队列
                    new_key = 'target_' + str(new_number)  # 获取最新的添加目标的键值
                    self.association_target[new_key] = self.current_frame.pop(noise_key)  # 真噪声/新目标目标，不做数据关联直接添加到最终字典
            # 大误差噪声点完成
            # 不止有噪声点，去噪完成，下面进行目标之间的数据关联
            if len(self.current_frame) > 0:
                af_association_matrix, af_cur_keys, pre_keys = self.build_association()  # 利用去噪后的前后帧数据构建的关联矩阵
                row, col = af_association_matrix.shape  # 提取关联矩阵的行和列数
                min_error_list = []  # 建立储存各种组合的误差之和的列表
                min_target_list = []  # 建立储存各种组合匹配对的目标id
                if row >= col:
                    self.pre = row
                    self.cur = col
                elif row < col:  # 当row<col时，row和col须调换位置
                    self.pre = col
                    self.cur = row
                indexer0 = itertools.cycle(range(row))  # 构建迭代变量
                indexer1 = itertools.cycle(range(col))  # 构建迭代变量
                for i in range(self.pre):
                    if row == col:
                        indexer0.__next__()  # 当矩阵为方阵时，需要执行这一步
                    sum_error = 0  # 初始化各目标匹配对的误差和
                    target_match = ''  # 初始化各目标匹配对的id组合
                    for j in range(self.cur):
                        index0 = indexer0.__next__()  # 迭代步骤
                        index1 = indexer1.__next__()  # 迭代步骤
                        sum_error += af_association_matrix[index0][index1]  # 误差和
                        target_match += str(index0) + ',' + str(index1) + ';'  # 目标匹配对
                    min_error_list.append(sum_error)  # 添加进误差和列表
                    min_target_list.append(target_match)  # 添加进目标目标匹配对列表
                indexer2 = itertools.cycle(range(row))
                indexer3 = itertools.cycle(range(col)[::-1])
                for i in range(self.pre):
                    if row == col:
                        indexer2.__next__()
                    sum_error = 0
                    target_match = ''
                    for j in range(self.cur):
                        index2 = indexer2.__next__()
                        index3 = indexer3.__next__()
                        sum_error += af_association_matrix[index2][index3]
                        target_match += str(index2) + ',' + str(index3) + ';'
                    min_error_list.append(sum_error)
                    min_target_list.append(target_match)
                min_target = min_error_list.index(min(min_error_list))  # 定位到最小误差的位置
                min_target_match = min_target_list[min_target]  # 找到最小误差的情况，字符串格式，'id,id;id,id;'的格式
                id_match = re.split(r';', min_target_match)  # 分离出各个id对
                for k in range(len(id_match) - 1):
                    target_correspond = re.split(r',', id_match[k])  # 分离出id
                    old_target_id = int(target_correspond[1])  # 提取当前帧中该目标所在的列号
                    old_target_key = af_cur_keys[old_target_id]  # 提取该目标的键值
                    new_target_id = int(target_correspond[0])  # 提取上一帧中该目标所在的列号
                    new_target_key = pre_keys[new_target_id]  # 提取匹配到的目标的键值
                    self.association_target[new_target_key] = self.current_frame.pop(old_target_key)  # 更改目标的键值，并放入字典
                # 上一帧目标完成
                # 还有部分小误差噪声
                if len(self.current_frame) > 0:
                    small_noise_frame = self.current_frame.copy()
                    for small_noise_key in small_noise_frame.keys():
                        new_number = sum_radar_target[-1] + 1  # 获取最新的添加目标的id值
                        sum_radar_target.append(new_number)  # 将最新的总目标数量更新到队列
                        new_key = 'target_' + str(new_number)  # 获取最新的添加目标的键值
                        self.association_target[new_key] = self.current_frame.pop(small_noise_key)  # 小噪声目标，添加进最终字典
            return self.association_target


# 声源数据提取函数
class SoundPreprocessing:
    def __init__(self, come_data):
        # 先读取数据，将数据读出来
        # sound = 'number;year,month,day,hour,minute,second,millisecond,confidence,x,y,z;'
        #                'year,month,day,hour,minute,second,millisecond,confidence,x,y,z;'  # target一般有2个，为正确目标、噪声
        self.split_data = re.split(r';', come_data)
        self.number = int(self.split_data[0])  # number表示目标的数量
        time_data = re.split(r',', self.split_data[1])
        hour = time_data[3]
        minute = time_data[4]
        second = time_data[5]
        millisecond = time_data[6]
        self.time = int(hour) * 3600 + int(minute) * 60 + int(second) + int(millisecond) / 1000

    def target_preprocess(self):
        sound_target_deque.clear()  # 将该队列清零，重新初始化
        for i in range(0, self.number):
            sound_time = self.time
            sound_data = self.split_data[i + 1]
            sound_target = str(sound_time) + ',' + sound_data
            sound_target_deque.append(sound_target)  # 将各目标数据以字符串形式依次放入待处理队列中
        return self.number

    def sound_target_preprocess(self):
        sound_target_precess.clear()  # 将该队列清零，重新初始化
        for i in range(0, self.number):
            sound_time = self.time
            sound_data = self.split_data[i + 1]
            sound_target = str(sound_time) + ',' + sound_data
            sound_target_precess.append(sound_target)  # 将各目标数据以字符串形式依次放入待处理队列中
        return self.number


# 声源目标提取函数
class SoundTargetPreprocessing:
    def __init__(self, come_data):
        # 处理单目标数据，输入单目标数据内容
        # 'time,year,month,day,hour,minute,second,millisecond,confidence,x,y,z;'
        sound_data = re.split(r',', come_data)
        sound_time = float(sound_data[0])
        confidence = sound_data[-4]
        x = sound_data[-3]
        y = sound_data[-2]
        z = sound_data[-1]
        self.time = sound_time
        self.confidence = confidence
        self.data = x + ',' + y + ',' + z
        self.x = x
        self.y = y
        self.z = z
        self.complete_data = sound_data[1] + ',' + sound_data[2] + ',' + sound_data[3] + ',' + sound_data[4] + ',' + \
                             sound_data[5] + ',' + sound_data[6] + ',' + sound_data[7] + ',' + sound_data[8] + ',' + \
                             sound_data[9] + ',' + sound_data[10] + ',' + sound_data[11]


# 平均声源目标函数
class AverSoundTargetPreprocessing:
    def __init__(self, come_data, x, y):
        # 处理单目标数据，输入单目标数据内容
        # 'year,month,day,hour,minute,second,millisecond,confidence,x,y,z;'
        sound_data = re.split(r',', come_data)
        sound_time = float(sound_data[0])
        confidence = sound_data[-4]
        z = sound_data[-1]
        self.time = sound_time
        self.confidence = confidence
        self.x = str(x)
        self.y = str(y)
        self.z = z
        self.data = self.x + ',' + self.y + ',' + z
        self.complete_data = sound_data[0] + ',' + sound_data[1] + ',' + sound_data[2] + ',' + sound_data[3] + ',' + \
                             sound_data[4] + ',' + sound_data[5] + ',' + sound_data[6] + ',' + sound_data[7] + ',' + \
                             self.x + ',' + self.y + ',' + z


# 声源目标的数据关联
class SoundDataAssociation:
    number: int  # 目标数量
    pre: int  # 中间变量
    cur: int  # 中间变量

    def __init__(self, come_data):
        self.data = come_data
        self.previous_frame = OrderedDict()  # 上一帧数据的字典
        self.current_frame = OrderedDict()  # 当前帧数据的字典
        self.history_frame = OrderedDict()  # 历史前几帧数据的字典
        self.noise_target_frame = OrderedDict()  # 创建一个新的字典，用来存储疑似噪声目标
        self.association_target = OrderedDict()  # 最终返回的已数据关联的字典
        self.noise_error = 2.0  # 噪声误差阈值，若目标位置误差大于该值，则判断其为噪声或是新的目标
        self.history_target_error = 1.0  # 旧目标误差阈值，若目标误差与历史帧中任何一帧中目标误差小于该值，则判断其为该历史目标

    def data_instantiate(self):
        # 将该帧中的目标数据实例化，并存成字典的格式
        self.number = SoundPreprocessing(self.data).target_preprocess()  # 对声源数据做预处理
        target_dictionary = OrderedDict()  # 创建一个空的有序字典，用来储存该帧中各目标数据
        for i in range(0, self.number):
            target = SoundTargetPreprocessing(sound_target_deque[i])  # 将雷达数据中所有目标实例化
            target_dictionary['target_' + str(i)] = target  # 各目标按顺序以键为target_0,1,2...存储目标数据值
        return target_dictionary

    def build_noise_association(self):
        # 构建噪声的关联矩阵，列为上一帧目标，行为当前帧目标
        row = len(self.history_frame)  # 关联矩阵的行数——上一帧数据的目标数量
        col = len(self.noise_target_frame)  # 关联矩阵的列数——当前帧数据的目标数量
        noise_association_matrix = np.zeros(shape=(row, col))  # 建立空的关联矩阵
        his_keys = []  # 建立存储上帧数据字典的键的列表
        for his_target_id in self.history_frame.keys():
            his_keys.append(his_target_id)  # 将上帧数据的键添加进列表中
        no_keys = []  # 建立存储当前数据字典的键的列表
        for no_target_id in self.noise_target_frame.keys():
            no_keys.append(no_target_id)  # 将当前数据的键添加进列表中
        for i in range(row):
            his_target = self.history_frame.get(his_keys[i])  # 依次提取上帧的目标数据
            his_position_x = float(his_target.x)
            his_position_y = float(his_target.y)
            for j in range(col):
                no_target = self.noise_target_frame.get(no_keys[j])  # 依次提取当前帧的目标数据
                no_position_x = float(no_target.x)
                no_position_y = float(no_target.y)
                error_x = abs(his_position_x - no_position_x)
                error_y = abs(his_position_y - no_position_y)
                error = (error_x ** 2 + error_y ** 2) ** 0.5  # 计算上帧其一目标与当前帧其一目标的位置误差
                noise_association_matrix[i][j] = error  # 将误差放入关联矩阵中
        return noise_association_matrix, no_keys, his_keys

    def build_association(self):
        # 构建关联矩阵，列为上一帧目标，行为当前帧目标
        row = len(self.previous_frame)  # 关联矩阵的行数——上一帧数据的目标数量
        col = len(self.current_frame)  # 关联矩阵的列数——当前帧数据的目标数量
        association_matrix = np.zeros(shape=(row, col))  # 建立空的关联矩阵
        pre_keys = []  # 建立存储上帧数据字典的键的列表
        for pre_target_id in self.previous_frame.keys():
            pre_keys.append(pre_target_id)  # 将上帧数据的键添加进列表中
        cur_keys = []  # 建立存储当前数据字典的键的列表
        for cur_target_id in self.current_frame.keys():
            cur_keys.append(cur_target_id)  # 将当前数据的键添加进列表中
        for i in range(row):
            pre_target = self.previous_frame.get(pre_keys[i])  # 依次提取上帧的目标数据
            pre_position_x = float(pre_target.x)
            pre_position_y = float(pre_target.y)
            for j in range(col):
                cur_target = self.current_frame.get(cur_keys[j])  # 依次提取当前帧的目标数据
                cur_position_x = float(cur_target.x)
                cur_position_y = float(cur_target.y)
                error_x = abs(pre_position_x - cur_position_x)
                error_y = abs(pre_position_y - cur_position_y)
                error = (error_x ** 2 + error_y ** 2) ** 0.5  # 计算上帧其一目标与当前帧其一目标的位置误差
                association_matrix[i][j] = error  # 将误差放入关联矩阵中
        return association_matrix, cur_keys, pre_keys

    def data_association(self):
        # 初始状态
        if len(unregistered_sound) == 0:
            # 全部为新目标
            self.association_target = self.data_instantiate()  # 首帧，直接实例+字典化，然后返回该帧数据
            sum_sound_target.append(self.number)  # 对目标数量队列初始化，第一帧的目标总数
            return self.association_target
        # 非初始状态
        elif len(unregistered_sound) > 0:
            # 1-5个目标，有一个目标位置变化不大，其他目标变化不明
            self.previous_frame = unregistered_sound[-1]  # 先提取上一帧的目标数据
            self.current_frame = self.data_instantiate()  # 对当前帧做实例+字典化
            be_association_matrix, be_cur_keys, pre_keys = self.build_association()
            min_col = np.min(be_association_matrix, axis=0)  # 找到关联矩阵的每一列中的最小值,生成一个列表
            noise_target_col = []  # 创建储存噪声/新目标目标点的集合，储存其列号
            for i in range(len(min_col)):  # 遍历关联矩阵的每一列
                if min_col[i] > self.noise_error:
                    noise_target_col.append(i)  # 若min的值都大于阈值，则判断为噪声，添加进噪声/新目标列表
            # 检验是否存在以往目标在上一帧中丢失的情况但在前几帧中一直存在的情况，保证精确匹配
            if len(noise_target_col) != 0:
                for m in range(len(noise_target_col)):  # 将所有疑似噪声重新弄一个字典
                    noise_target_id = noise_target_col[m]  # 提取噪声/目标所在的列号
                    noise_target_key = be_cur_keys[noise_target_id]  # 提取噪声/目标的键值
                    self.noise_target_frame[noise_target_key] = self.current_frame[noise_target_key]  # 添加疑似噪声目标进新字典
                for n in range(len(unregistered_sound) - 1):  # 从前几帧的历史帧中依次寻找
                    self.history_frame = unregistered_sound[len(unregistered_sound) - n - 1]  # 从历史帧中获取数据，检验是否存在相似目标
                    if self.noise_target_frame == 0 or n == 4:  # 若所有疑似噪声已经匹配完毕，或已经找了4帧，则退出循环
                        break
                    noise_association_matrix, no_keys, his_keys = self.build_noise_association()  # 构建疑似噪声和历史帧的关联矩阵
                    min_target_error = np.min(noise_association_matrix, axis=0)  # 找到该关联矩阵的每一列中的最小值,生成一个列表
                    for i in range(0, len(min_target_error)):  # 遍历关联矩阵的每一列
                        if min_target_error[i] < self.history_target_error:  # 若误差小于阈值
                            old_noise_target_key = no_keys[i]  # 获取该疑似旧目标的疑似噪声的键值
                            no_row, no_col = noise_association_matrix.shape  # 提取关联矩阵的行和列数
                            for j in range(0, no_row):  # 在矩阵的这一列中寻找
                                if noise_association_matrix[j][i] == min_target_error[i]:  # 若找到该最小误差值
                                    new_his_target_id = j  # 找到旧目标的行号
                                    new_his_target_key = his_keys[new_his_target_id]  # 通过其列号找到该旧目标的键值
                                    self.association_target[new_his_target_key] = self.noise_target_frame.pop(
                                        old_noise_target_key)  # 更改目标的键值，并放入最终字典
                                    del self.current_frame[old_noise_target_key]  # 匹配完成，将该目标从当前帧字典中删去
                                    break
            # 前历史帧目标完成
            if len(self.noise_target_frame) > 0:
                for noise_key in self.noise_target_frame.keys():
                    new_number = sum_sound_target[-1] + 1  # 获取最新的添加目标的id值
                    sum_sound_target.append(new_number)  # 将最新的总目标数量更新到队列
                    new_key = 'target_' + str(new_number)  # 获取最新的添加目标的键值
                    self.association_target[new_key] = self.current_frame.pop(noise_key)  # 真噪声/新目标目标，不做数据关联直接添加到最终字典
            # 大误差噪声点完成
            # 不止有噪声点，去噪完成，下面进行目标之间的数据关联
            if len(self.current_frame) > 0:
                af_association_matrix, af_cur_keys, pre_keys = self.build_association()  # 利用去噪后的前后帧数据构建的关联矩阵
                row, col = af_association_matrix.shape  # 提取关联矩阵的行和列数
                min_error_list = []  # 建立储存各种组合的误差之和的列表
                min_target_list = []  # 建立储存各种组合匹配对的目标id
                if row >= col:
                    self.pre = row
                    self.cur = col
                elif row < col:  # 当row<col时，row和col须调换位置
                    self.pre = col
                    self.cur = row
                indexer0 = itertools.cycle(range(row))  # 构建迭代变量
                indexer1 = itertools.cycle(range(col))  # 构建迭代变量
                for i in range(self.pre):
                    if row == col:
                        indexer0.__next__()  # 当矩阵为方阵时，需要执行这一步
                    sum_error = 0  # 初始化各目标匹配对的误差和
                    target_match = ''  # 初始化各目标匹配对的id组合
                    for j in range(self.cur):
                        index0 = indexer0.__next__()  # 迭代步骤
                        index1 = indexer1.__next__()  # 迭代步骤
                        sum_error += af_association_matrix[index0][index1]  # 误差和
                        target_match += str(index0) + ',' + str(index1) + ';'  # 目标匹配对
                    min_error_list.append(sum_error)  # 添加进误差和列表
                    min_target_list.append(target_match)  # 添加进目标目标匹配对列表
                indexer2 = itertools.cycle(range(row))
                indexer3 = itertools.cycle(range(col)[::-1])
                for i in range(self.pre):
                    if row == col:
                        indexer2.__next__()
                    sum_error = 0
                    target_match = ''
                    for j in range(self.cur):
                        index2 = indexer2.__next__()
                        index3 = indexer3.__next__()
                        sum_error += af_association_matrix[index2][index3]
                        target_match += str(index2) + ',' + str(index3) + ';'
                    min_error_list.append(sum_error)
                    min_target_list.append(target_match)
                min_target = min_error_list.index(min(min_error_list))  # 定位到最小误差的位置
                min_target_match = min_target_list[min_target]  # 找到最小误差的情况，字符串格式，'id,id;id,id;'的格式
                id_match = re.split(r';', min_target_match)  # 分离出各个id对
                for k in range(len(id_match) - 1):
                    target_correspond = re.split(r',', id_match[k])  # 分离出id
                    old_target_id = int(target_correspond[1])  # 提取当前帧中该目标所在的列号
                    old_target_key = af_cur_keys[old_target_id]  # 提取该目标的键值
                    new_target_id = int(target_correspond[0])  # 提取上一帧中该目标所在的列号
                    new_target_key = pre_keys[new_target_id]  # 提取匹配到的目标的键值
                    self.association_target[new_target_key] = self.current_frame.pop(
                        old_target_key)  # 更改目标的键值，并放入字典
                # 上一帧目标完成
                # 还有部分小误差噪声
                if len(self.current_frame) > 0:
                    small_noise_frame = self.current_frame.copy()
                    for small_noise_key in small_noise_frame.keys():
                        new_number = sum_sound_target[-1] + 1  # 获取最新的添加目标的id值
                        sum_sound_target.append(new_number)  # 将最新的总目标数量更新到队列
                        new_key = 'target_' + str(new_number)  # 获取最新的添加目标的键值
                        self.association_target[new_key] = self.current_frame.pop(small_noise_key)  # 小噪声目标，添加进最终字典
            return self.association_target


# 获取系统时间，并将其转成相对时间
def system_time():
    now_time = datetime.now()
    time_dt = time.strptime(str(now_time), '%Y-%m-%d %H:%M:%S.%f')
    time_ms = str(now_time)[-6:-3]
    sys_time = time_dt.tm_hour * 3600 + time_dt.tm_min * 60 + time_dt.tm_sec + int(time_ms) / 1000
    return sys_time


# 将相对时间转换成准确时间，转换成时分秒毫秒格式
def time_conversion(opposite_time):
    now_time = datetime.now()
    time_dq = time.strptime(str(now_time), '%Y-%m-%d %H:%M:%S.%f')
    year = time_dq.tm_year
    m = time_dq.tm_mon
    mon = format(float(m) / 100, '.2f')
    month = mon[2:4]
    d = time_dq.tm_mday
    da = format(float(d) / 100, '.2f')
    day = da[2:4]
    h = int(opposite_time / 3600)
    ho = format(float(h) / 100, '.2f')
    hour = ho[2:4]
    m = int((opposite_time % 3600) / 60)
    mi = format(float(m) / 100, '.2f')
    minute = mi[2:4]
    s = int(opposite_time % 60)
    sec = format(float(s) / 100, '.2f')
    second = sec[2:4]
    ms = format(opposite_time % 1, '.3f')
    millisecond = ms[2:5]
    accurate_time = str(str(year) + ',' + month + ',' + day + ','
                        + hour + ',' + minute + ',' + second + ',' + millisecond)
    return accurate_time


# 同步时间生成函数
class SyncTime:
    radar_sync_time: float

    def __init__(self, sync_period):
        self.sync_period = sync_period

    def radar__sync__time(self):
        # 得到同步时间
        self.radar_sync_time = float(format(radar_sync_time_queue[-1] + self.sync_period, '.3f'))
        radar_sync_time_queue.append(self.radar_sync_time)
        return self.radar_sync_time

    def sound__sync__time(self):
        # 得到同步时间
        self.sound_sync_time = float(format(sound_sync_time_queue[-1] + self.sync_period, '.3f'))
        sound_sync_time_queue.append(self.sound_sync_time)
        return self.sound_sync_time


# 雷达数据时间配准函数
class RadarRegistration:
    valid_radar_time: str
    valid_radar_data: str

    def __init__(self, ra_flag, sy_time, radar_communication_deque):
        self.sync_time = sy_time
        self.radar_flag = ra_flag
        self.radar_communication_deque = radar_communication_deque

    def radar_registration(self):
        # 检查雷达通信队列是否更新；若有更新，则保存接收到的数据，否则队列不更新，直接做时间配准
        if self.radar_flag == 0:  # 未更新
            pass
        elif self.radar_flag == 1:  # 有更新
            # 先执行数据关联程序，输入最新的雷达数据，返回一个字典类型的数据，即包含所有目标的一帧数据
            radar_sub_data = RadarDataAssociation(self.radar_communication_deque[-1]).data_association()
            unregistered_radar.append(radar_sub_data)  # 将最新更新的雷达数据放进未同步的雷达双边队列中

        if len(unregistered_radar) == 0:  # 若未配准雷达队列为空，空则返回空数据
            self.valid_radar_time = time_conversion(self.sync_time)
            self.valid_radar_data = 'None'  # 雷达未检测到目标，目标数据返回None
            radar_data_data = str(self.valid_radar_time) + ',' + str(self.valid_radar_data)  # 将同步时间和数据组合起来
            radar_data_queue.append(radar_data_data)  # 将处理后的数据放入同步后的雷达队列中
        elif len(unregistered_radar) > 0:  # 未配准雷达队列不为空，可以进行配准
            valid_radar = RadarSyncData(unregistered_radar, self.sync_time)  # 得到配准后的有效数据
            self.valid_radar_time = time_conversion(self.sync_time)
            self.valid_radar_data = valid_radar.radar_registration()
            radar_data_data = str(self.valid_radar_time) + ',' + str(self.valid_radar_data)
            radar_data_queue.append(radar_data_data)  # 将数据放入雷达队列中


# 声源数据时间配准函数
class SoundRegistration:  # 声源时间配准
    valid_sound_time: str
    valid_sound_data: str

    def __init__(self, sound_flag, sy_time, sound_communication_deque):
        self.sync_time = sy_time
        self.sound_flag = sound_flag
        self.sound_communication_deque = sound_communication_deque

    def sound_registration(self):
        # 若有更新，则保存接收到的数据，否则未配准队列不更新，直接做时间配准
        if self.sound_flag == 0:
            pass
        elif self.sound_flag == 1:
            sound_sub_data = SoundDataAssociation(self.sound_communication_deque[-1]).data_association()
            unregistered_sound.append(sound_sub_data)  # 将声源数据先放进未同步的声源双边队列中

        if len(unregistered_sound) == 0:  # 判断声源队列为空，空则返回空元素的队列
            self.valid_sound_time = time_conversion(self.sync_time)
            self.valid_sound_data = 'None'  # 声源未检测到目标，则返回None
            sound_data_data = str(self.valid_sound_time) + ',' + str(self.valid_sound_data)
            sound_data_queue.append(sound_data_data)  # 将数据放入声源队列中
        elif len(unregistered_sound) > 0:  # 判断队列是否为空，为空则初始化不进行配准
            valid_sound = SoundSyncData(unregistered_sound, self.sync_time)  # 得到配准后的有效数据
            self.valid_sound_time = time_conversion(self.sync_time)
            self.valid_sound_data = valid_sound.sound_registration()
            sound_data_data = str(self.valid_sound_time) + ',' + str(self.valid_sound_data)
            sound_data_queue.append(sound_data_data)  # 将数据放入声源队列中
        return sound_data_queue


class TcpLogic(tcp_udp_web_ui.ToolsUi):
    def __init__(self, num):
        super(TcpLogic, self).__init__(num)

        self.tcp_socket = None  # 雷达socket通信线程
        self.laser_tcp_socket = None  # 激光socket通信线程

        self.client_th = None     # 雷达处理数据线程
        self.client_laser_th = None # 激光处理数据线程
        self.fusion_thread_flag = True  # 用于关闭线程
        self.radar_time_thread_flag = True  # 用于关闭线程
        self.laser_time_thread_flag = True  # 用于关闭线程
        self.radar_filter_thread_flag = True  # 用于关闭线程
        self.laser_filter_thread_flag = True  # 用于关闭线程
        self.laser_close_flag = False  # 激光连接失败标志
        self.single_fusion_flag = False  # 融合去除激光点标志
        self.single_fusion_number = 0  # 融合去除激光点标志数量位
        self.single_fusion_flag1 = False  # 融合去除激光点标志

        self.fusion_history_point_list = []
        self.radar_history_point_list = []
        self.laser_history_point_list = []
        self.link = False  # 用于标记是否开启了连接
        self.radar_turn_speed = 5  # 云台速度默认5级
        self.corner_x = -5.8
        self.corner_y = 50.8
        global radar_filter_communication
        global sound_filter_communication
        self.init_time = system_time()  # 获取同步初始时间
        self.radar_init_time = self.init_time
        radar_sync_time_queue.append(self.radar_init_time)  # 首次运行，先将初始同步时间放入同步时间队列中
        self.sound_init_time = self.init_time
        sound_sync_time_queue.append(self.sound_init_time)  # 首次运行，先将初始同步时间放入同步时间队列中
        self.radar_averT = 0.125  # 雷达平均周期
        self.sound_averT = 1.000  # 声源平均周期
        self.sync_averT = 0.500  # 同步平均周期

        self.radar_fusion_time = ''
        self.laser_fusion_time = ''

        self.radar_object_list_ui = []
        self.fusion_object_list_ui = []

    # 雷达客户端连接
    def tcp_client_start(self):
        """
        功能函数，TCP客户端连接其他服务端的方法
        :return:
        """
        # print("client start")
        self.tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        try:
            address = (str(self.lineEdit_ip_send.text()), int(self.lineEdit_port.text()))
        except Exception as ret:
            msg = '请检查穿墙雷达IP，目标端口\n'
            self.signal_write_rader_msg.emit(msg)
        else:
            try:
                msg = '正在连接穿墙雷达服务器\n'
                self.signal_write_rader_msg.emit(msg)
                self.tcp_socket.connect(address)
            except Exception as ret:
                msg = '无法连接穿墙雷达服务器\n'
                self.signal_write_rader_msg.emit(msg)
            else:
                self.client_th = threading.Thread(target=self.tcp_client_concurrency, args=(address,), daemon=True)
                self.client_th.start()

                msg = 'TCP穿墙雷达客户端已连接IP:%s端口:%s\n' % address
                self.signal_write_rader_msg.emit(msg)

    # 激光客户端连接
    def laser_tcp_client_start(self):
        """
        功能函数，TCP客户端连接其他服务端的方法
        :return:
        """
        self.laser_tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        try:
            address = (str(self.lineEdit_laser_ip_send.text()), int(self.lineEdit_laser_port.text()))
            # address = ('192.168.4.100', 65501)
        except Exception as ret:
            msg = '请检查激光IP，目标端口\n'
            self.signal_write_laser_msg.emit(msg)
        else:
            try:
                msg = '正在连接激光服务器\n'
                self.signal_write_laser_msg.emit(msg)
                self.laser_tcp_socket.connect(address)
                self.laser_tcp_socket.send(b'1')
            except Exception as ret:
                self.laser_close_flag = True
                msg = '无法连接激光服务器\n'
                self.signal_write_laser_msg.emit(msg)

            else:

                self.client_laser_th = threading.Thread(target=self.laser_tcp_client_concurrency, args=(address,),
                                                        daemon=True)
                self.client_laser_th.start()

                self.laser_time_thread_flag = True
                sound_registration_thread = threading.Thread(target=self.sound_thread, daemon=True)  # 定义时间配准线程
                sound_registration_thread.start()
                msg = 'TCP激光客户端已连接IP:%s端口:%s\n' % address
                self.signal_write_laser_msg.emit(msg)

    # 雷达 功能函数，用于TCP客户端创建子线程的方法，阻塞式接收
    def tcp_client_concurrency(self, address):
        year = date.today().year
        month = date.today().month
        day = date.today().day
        hour = datetime.now().hour
        minute = datetime.now().minute
        second = datetime.now().second
        radar_name = "./data/Radar/Radar-" + str(year) + "-" + str(month) + "-" + str(day) + " " + str(hour) + "-" + str(minute) + "-" + str(second) + ".txt"
        self.radar_file = open(radar_name, "w")
        open("radar_recv_data.txt", 'w').close()

        while True:
            try:
                recv_msg = self.tcp_socket.recv(12)  # 9位数据
            except OSError:
                msg = '从服务器断开连接\n'
                self.signal_write_rader_msg.emit(msg)
                break
            else:
                radar_data = recv_msg
                # 此处处理数据
                if (radar_data[0:5] == b'\xa5\x5a\xb5\x10\x30'):
                    received_data_length = len(radar_data)  # 已经收到了这么长的数据

                    data_wall_length_hex = radar_data[5:9]
                    pack_length = struct.unpack('i', bytes(data_wall_length_hex))[0] + 10

                    radar_object_data = bytes(''.encode("utf8"))
                    radar_object_data += radar_data
                    left_received_len = pack_length - received_data_length  # 修改？
                    radar_object_data += RecvN(self.tcp_socket, left_received_len)

                    # write radar file
                    self.radar_file.write(radar_object_data.hex())
                    self.radar_file.write("\n")

                    print("开始分析雷达目标数据")
                    object_list, data_number_radar_dec = dataRadarAnalysis(radar_object_data)  # 雷达目标处理函数[('0.430', '10.013')]

                    radar_recv_file = open('radar_recv_data.txt', mode='a+')
                    radar_recv_file.write(data_number_radar_dec + '\n')
                    radar_communication.put(data_number_radar_dec)

                    # self.history_fusion_point(object_list, self.radar_history_point_list)
                    self.radar_object_list_ui = object_list
                    self.signal_radar_flush.emit(object_list)
                    msg = '---------------------------------------'+'\n'
                    for point_X, point_Y in object_list:
                        object_radar_x = str(float(point_X))
                        object_radar_y = str(float(point_Y))
                        msg = msg + '(' + object_radar_x + ',' + object_radar_y + ')'
                    msg = msg + '\n'
                    self.signal_write_rader_msg.emit(msg)

                    print("雷达目标数据分析成功")

                elif (radar_data[0:5] == b'\xa5\x5a\xb5\x10\x31'):
                    print("收到墙体数据")
                    received_data_length = len(radar_data)  # 已经收到了这么长的数据
                    receiverBufsize = 4096
                    # print("receiverBufsize:",receiverBufsize)
                    data_wall_length_hex = radar_data[5:9]
                    pack_length = struct.unpack('i', bytes(data_wall_length_hex))[0] + 10
                    radar_wall_data = bytes(''.encode("utf8"))
                    radar_wall_data += radar_data
                    left_received_len = pack_length - received_data_length  # 修改？
                    if receiverBufsize < left_received_len:
                        # radar_wall_data = bytes(''.encode("utf8"))
                        left_pack_length = left_received_len

                        while left_pack_length > 0:
                            # print("left_pack_length",left_pack_length)
                            if left_pack_length > receiverBufsize:
                                body_part = self.tcp_socket.recv(receiverBufsize)
                            else:
                                body_part = self.tcp_socket.recv(left_pack_length)
                            # print("body_part len",len(body_part))
                            radar_wall_data += body_part
                            left_pack_length -= len(body_part)
                    else:
                        radar_wall_data = self.tcp_socket.recv(pack_length)
                    # print("total len:",len(radar_wall_data))

                    # write radar file
                    self.radar_file.write(radar_wall_data.hex())
                    self.radar_file.write("\n")

                    print("开始墙体分析数据")
                    client_laser_Wall = threading.Thread(target=self.dataRadarWallAnalysis, args=(radar_wall_data,), daemon=True)
                    client_laser_Wall.start()
                    print("墙体数据分析成功")

                elif (radar_data[0:5] == b'\xa5\x5a\xb5\x10\x20'):
                    print("收到图片数据")
                    received_data_length = len(radar_data)  # 已经收到了这么长的数据
                    receiverBufsize = 65536
                    # print("receiverBufsize:",receiverBufsize)
                    data_wall_length_hex = radar_data[5:9]
                    pack_length = struct.unpack('i', bytes(data_wall_length_hex))[0] + 10
                    # print("图片数据总共需要长度为:",pack_length)
                    radar_image_data = bytes(''.encode("utf8"))
                    radar_image_data += radar_data
                    left_received_len = pack_length - received_data_length
                    if receiverBufsize < left_received_len:
                        left_pack_length = left_received_len

                        while left_pack_length > 0:
                            # print("left_pack_length",left_pack_length)
                            if left_pack_length > receiverBufsize:
                                body_part = self.tcp_socket.recv(receiverBufsize)
                            else:
                                body_part = self.tcp_socket.recv(left_pack_length)
                            # print("body_part len",len(body_part))
                            radar_image_data += body_part
                            left_pack_length -= len(body_part)
                    else:
                        radar_image_data = self.tcp_socket.recv(pack_length)
                    # print("total len:",len(radar_image_data))

                    assert len(radar_image_data) == pack_length, "长度不相等"

                    # write radar file
                    self.radar_file.write(radar_image_data.hex())
                    self.radar_file.write("\n")

                    print("开始分析图像数据")
                    imgagesName = dataImageAnalysis(radar_image_data)  # 相机图像处理函数
                    # self.radar_images = cv2.imread(imgagesName)
                    # self.radar_images = cv2.imread('2019-8-19-11-18-33-520' + '.png')
                    print("图像数据分析成功")

                elif (radar_data[0:5] == b'\xa5\x5a\xb5\x10\xa2'):
                    data_respond_symmbol_hex = radar_data[9:11]
                    if data_respond_symmbol_hex == b'\x03\x01':  # 雷达控制应答
                        msg = '雷达正在扫描\n'
                        self.signal_write_rader_msg.emit(msg)

                        self.radar_time_thread_flag = True
                        radar_registration_thread = threading.Thread(target=self.radar_thread, daemon=True)  # 定义时间配准线程
                        radar_registration_thread.start()

                        if self.radar_filter_thread_flag == False:
                            self.radar_filter_thread_flag = True
                            radar_filter_thread = threading.Thread(target=self.radar_filter, daemon=True)  # 雷达滤波线程
                            radar_filter_thread.start()
                    elif data_respond_symmbol_hex == b'\x03\x02':  # 雷达控制应答
                        msg = '雷达停止扫描\n'
                        self.signal_write_rader_msg.emit(msg)

                        self.radar_init_time = radar_sync_time_queue[-1]
                        self.radar_time_thread_flag = False
                        self.radar_filter_thread_flag = False
                        self.single_fusion_flag = False  # 融合去除激光点标志
                        self.single_fusion_number = 0  # 融合去除激光点标志数量位
                        queue_radar_target.queue.clear()

                        # self.tcp_socket.setblok(1)
                        # while True:
                        #     try:
                        #         recv_clear_msg = self.tcp_socket.recv(1024)  #
                        #     except  BlockingIOError as e:
                        #         msg = '缓存区已经清除\n'
                        #         self.signal_write_rader_msg.emit(msg)
                        #         break
                        # self.tcp_socket.setblok(0)

                    elif data_respond_symmbol_hex == b'\x04\x0f':  # 云台控制应答
                        msg = '云台控制应答成功\n'
                        self.signal_write_rader_msg.emit(msg)
                    elif data_respond_symmbol_hex == b'\x05\x0f':  # 墙体融合应答
                        msg = '雷达墙体融合应答成功\n'
                        self.signal_write_rader_msg.emit(msg)
                    elif data_respond_symmbol_hex == b'\x20\x0f':  # 获取相机图像应答
                        msg = '获取相机图像应答成功\n'
                        self.signal_write_rader_msg.emit(msg)
                    elif data_respond_symmbol_hex == b'\x30\x0f':  # 目标数据上传
                        msg = '目标数据上传应答成功\n'
                        self.signal_write_rader_msg.emit(msg)
                    elif data_respond_symmbol_hex == b'\x31\x0f':  # 墙体数据上传应答
                        msg = '墙体数据上传应答成功\n'
                        self.signal_write_rader_msg.emit(msg)

                    self.radar_file.write(radar_data.hex())
                    self.radar_file.write("\n")

    # 激光 功能函数，用于TCP客户端创建子线程的方法，阻塞式接收
    def laser_tcp_client_concurrency(self, address):
        """
        功能函数，用于TCP客户端创建子线程的方法，阻塞式接收
        :return:
        """
        year = date.today().year
        month = date.today().month
        day = date.today().day
        hour = datetime.now().hour
        minute = datetime.now().minute
        second = datetime.now().second
        laser_name = "./data/Laser/Laser-" + str(year) + "-" + str(month) + "-" + str(day) + " " + str(hour) + "-" + str(
            minute) + "-" + str(second) + ".txt"
        self.laser_file = open(laser_name, "w")
        open("sound_recv_data.txt", 'w').close()

        while True:
            try:
                recv_msg = self.laser_tcp_socket.recv(256)
            except Exception:
                msg = '激光从服务器断开连接\n'
                self.signal_write_laser_msg.emit(msg)
                break
            else:
                # if recv_msg:
                msg = recv_msg.hex()
                self.laser_file.write(msg)
                self.laser_file.write("\n")

                self.split_laser_buf(recv_msg)

    # 处理激光数据
    def split_laser_buf(self, buf):
        proto = communication_protocol(buf)

        object_list = []
        msg = '---------------------------------------'+'\n'
        laser_data = str(proto.laserLength) + ';'

        for j in range(proto.laserLength):

            proto_x = proto.point[j].x
            proto.point[j].x = self.corner_x + 3.51 + proto.point[j].y
            proto.point[j].y = self.corner_y + 4.16 - proto_x

            msg = msg + '(' + format(proto.point[j].x,'.3f') + ',' + format(proto.point[j].y,'.3f') + ')'
            object_point = (proto.point[j].x, proto.point[j].y)
            object_list.append(object_point)

            self.laser_place = format(proto.point[j].x,'.3f')  + ',' + format(proto.point[j].y,'.3f')

            laser_data = laser_data + str(proto.year) + ',' + str(proto.month) + ',' + str(proto.day) + ',' + str(proto.hour) + ',' + \
                         str(proto.minute) + ',' + str(proto.second) + ',' + str(proto.millisecond) + ',' + \
                         str(proto.point[j].creditability) + ',' + str(proto.point[j].x) + ',' + str(
                proto.point[j].y) + ',' + str(proto.point[j].z) + ';'


        self.signal_write_laser_msg.emit(msg + '\n')  # 激光浏览框显示
        self.signal_laser_flush.emit(object_list)  # 画布画出激光点
        self.history_fusion_point(object_list, self.laser_history_point_list)
        radar_data_file = open("./data/Laser/Laser-object.txt", 'a+')
        radar_data_file.write(laser_data + "\n")
        radar_data_file.close()

        sound_communication.put(laser_data)

        sound_recv_file = open('sound_recv_data.txt', mode='a+')
        sound_recv_file.write(laser_data + '\n')

    # 雷达指令发送
    def radar_scan_send(self):
        """
        功能函数，用于TCP服务端和TCP客户端发送消息
        :return: None
        """
        if self.link is False:
            msg = '请选择服务，并点击连接网络\n'
            self.signal_write_rader_msg.emit(msg)
        else:
            try:
                if self.comboBox_radar_scan.currentIndex() == 0:
                    radar_steart = b'\xa5\x5a\x10\xb5\x03\x01\x00\x00\x00\xa2\x6b'
                    self.tcp_socket.send(radar_steart)
                    msg = 'TCP穿墙雷达客户端已发送停止命令\n'
                    self.signal_write_rader_msg.emit(msg)
                elif self.comboBox_radar_scan.currentIndex() == 1:
                    radar_steart = b'\xa5\x5a\x10\xb5\x03\x01\x00\x00\x00\xa0\x69'
                    self.tcp_socket.send(radar_steart)
                    msg = 'TCP穿墙雷达客户端已发送启动命令\n'
                    self.signal_write_rader_msg.emit(msg)
                elif self.comboBox_radar_scan.currentIndex() == 2:
                    radar_steart = b'\xa5\x5a\x10\xb5\x05\x01\x00\x00\x00\x01\xcc'
                    self.tcp_socket.send(radar_steart)
                    msg = 'TCP穿墙雷达客户端已发送开始融合命令1\n'
                    self.signal_write_rader_msg.emit(msg)
                elif self.comboBox_radar_scan.currentIndex() == 3:
                    radar_steart = b'\xa5\x5a\x10\xb5\x05\x01\x00\x00\x00\x02\xcd'
                    self.tcp_socket.send(radar_steart)
                    msg = 'TCP穿墙雷达客户端已发送开始融合命令2\n'
                    self.signal_write_rader_msg.emit(msg)
                elif self.comboBox_radar_scan.currentIndex() == 4:
                    radar_steart = b'\xa5\x5a\x10\xb5\x05\x01\x00\x00\x00\x00\xcb'
                    self.tcp_socket.send(radar_steart)
                    msg = 'TCP穿墙雷达客户端已发送获取图像命令\n'
                    self.signal_write_rader_msg.emit(msg)
                else:
                    msg = 'TCP穿墙雷达客户端命令错误，请重新输入\n'
                    self.signal_write_rader_msg.emit(msg)
            except Exception as ret:
                msg = '发送失败\n'
                self.signal_write_rader_msg.emit(msg)

    # 雷达下拉框云台指令发送
    def radar_turn_send(self):
        """
        功能函数，用于TCP服务端和TCP客户端发送消息
        :return: None
        """
        if self.link is False:
            msg = '请选择服务，并点击连接网络\n'
            self.signal_write_rader_msg.emit(msg)
        else:
            try:
                if self.comboBox_radar_turn.currentIndex() == 0:
                    radar_steart = b'\xa5\x5a\x10\xb5\x04\x07\x00\x00\x00\xff\x01\x00\x00\x00\x00\x01\xd1'
                    self.tcp_socket.send(radar_steart)
                    msg = '穿墙雷达云台停止转动\n'
                    self.signal_write_rader_msg.emit(msg)
                elif self.comboBox_radar_turn.currentIndex() == 1:
                    radar_steart = b'\xa5\x5a\x10\xb5\x04\x07\x00\x00\x00\xff\x01\x00\x04\x05\x00\x0a\xe3'
                    self.tcp_socket.send(radar_steart)
                    msg = '穿墙雷达云台水平向左转\n'
                    self.signal_write_rader_msg.emit(msg)
                elif self.comboBox_radar_turn.currentIndex() == 2:
                    radar_steart = b'\xa5\x5a\x10\xb5\x04\x07\x00\x00\x00\xff\x01\x00\x02\x05\x00\x08\xdf'
                    self.tcp_socket.send(radar_steart)
                    msg = '穿墙雷达云台水平向右转\n'
                    self.signal_write_rader_msg.emit(msg)
                elif self.comboBox_radar_turn.currentIndex() == 3:
                    radar_steart = b'\xa5\x5a\x10\xb5\x04\x07\x00\x00\x00\xff\x01\x00\x08\x00\x05\x0e\xeb'
                    self.tcp_socket.send(radar_steart)
                    msg = '穿墙雷达云台俯仰向上转动\n'
                    self.signal_write_rader_msg.emit(msg)
                elif self.comboBox_radar_turn.currentIndex() == 4:
                    radar_steart = b'\xa5\x5a\x10\xb5\x04\x07\x00\x00\x00\xff\x01\x00\x10\x00\x05\x16\xfb'
                    self.tcp_socket.send(radar_steart)
                    msg = '穿墙雷达云台俯仰向下转动\n'
                    self.signal_write_rader_msg.emit(msg)
                else:
                    msg = 'TCP穿墙雷达客户端命令错误，请重新输入\n'
                    self.signal_write_rader_msg.emit(msg)
            except Exception as ret:
                msg = '发送失败\n'
                self.signal_write_rader_msg.emit(msg)

    # 云台发送加减速指令发送
    def radar_turn_add_reduce(self):
        radar_turn_speed_hex = struct.pack('b', self.radar_turn_speed)
        radar_turn_index = self.comboBox_radar_turn.currentIndex()
        radar_start_2 = b''
        radar_start_1 = b'\x10\xb5\x04\x07\x00\x00\x00\xff'
        if radar_turn_index != 0:
            if radar_turn_index == 1:
                radar_start_2_1 = b'\x01\x00\x04' + radar_turn_speed_hex + b'\x00'
                radar_start_2 = radar_start_1 + radar_start_2_1 + struct.pack('B', sum(radar_start_2_1) & 0xff)
            elif radar_turn_index == 2:
                radar_start_2_1 = b'\x01\x00\x02' + radar_turn_speed_hex + b'\x00'
                radar_start_2 = radar_start_1 + radar_start_2_1 + struct.pack('B', sum(radar_start_2_1) & 0xff)
            elif radar_turn_index == 3:
                radar_start_2_1 = b'\x01\x00\x08' + b'\x00' + radar_turn_speed_hex
                radar_start_2 = radar_start_1 + radar_start_2_1 + struct.pack('B', sum(radar_start_2_1) & 0xff)
            elif radar_turn_index == 4:
                radar_start_2_1 = b'\x01\x00\x10' + b'\x00' + radar_turn_speed_hex
                radar_start_2 = radar_start_1 + radar_start_2_1 + struct.pack('B', sum(radar_start_2_1) & 0xff)

            radar_steart = b'\xa5\x5a' + radar_start_2 + struct.pack('B', sum(radar_start_2) & 0xff)
            print(radar_steart.hex())
            self.tcp_socket.send(radar_steart)
            msg = '穿墙雷达云台旋转速度:' + str(self.radar_turn_speed) + '\n'
            self.signal_write_rader_msg.emit(msg)
        else:
            msg = '穿墙雷达云台不可旋转\n'
            self.signal_write_rader_msg.emit(msg)

    # 关闭tcp通信
    def tcp_close(self):
        detect_msg = "检测人数:"
        self.label_detect_number_people.setText(detect_msg)
        real_msg = "房长:"
        self.label_real_number_people.setText(real_msg)

        error_msg = "房宽:"
        self.label_error_number_people.setText(error_msg)

        real_msg = "隔间长:"
        self.label_sub_house_length.setText(real_msg)

        error_msg = "隔间宽:"
        self.label_sub_house_width.setText(error_msg)
        try:
            self.fusion_thread_flag = False
            self.laser_filter_thread_flag = False
            self.radar_filter_thread_flag = False

            self.radar_init_time = radar_sync_time_queue[-1]
            self.radar_time_thread_flag = False
            self.sound_init_time = sound_sync_time_queue[-1]
            self.laser_time_thread_flag = False
            queue_laser_target.queue.clear()
            queue_radar_target.queue.clear()
        except Exception:
            pass
        try:
            self.tcp_socket.shutdown(socket.SHUT_RDWR)
            self.tcp_socket.close()  # socket关闭连接之后，client_th线程关闭

        except Exception:
            pass
        try:
            self.laser_tcp_socket.shutdown(socket.SHUT_RDWR)
            self.laser_tcp_socket.close()  # socket关闭连接之后，client_laser_th线程关闭
        except Exception:
            pass
        try:
            self.radar_file.close()  # 关闭文件
        except Exception:
            pass
        try:
            self.laser_file.close()  # 关闭文件
        except Exception:
            pass
        try:
            print("client_th", self.client_th.isAlive())
            print("client_laser_th", self.client_laser_th.isAlive())
        except Exception:
            pass
        if self.link is True:
            msg = '已断开网络\n'
            self.signal_write_rader_msg.emit(msg)
            self.signal_write_laser_msg.emit(msg)

    def clear_queue(self):
        detect_msg = "检测人数:"
        self.label_detect_number_people.setText(detect_msg)
        sound_aver_data.clear()
        queue_laser_target.queue.clear()
        queue_radar_target.queue.clear()

    # 雷达数据滤波
    def radar_filter(self):
        open("radar_filter_data.txt", 'w').close()
        collect_radar_data.clear()
        sure_target_deque.clear()
        sure_target_possible.clear()
        noise_target_deque.clear()
        miss_data_number = 0
        while self.radar_filter_thread_flag is True:
            filter_output = deque(maxlen=10)
            self.get_flag = 0
            try:
                self.data = radar_communication.get(timeout=0.5)
            except queue.Empty:
                out_data = 'No Radar Data'
                queue_radar_target.put(out_data)
                radar_file = open('radar_filter_data.txt', mode='a')
                radar_file.write(out_data + '\n')
                continue
            if len(collect_radar_data) < 19:
                # 初始情况，只添加不做处理
                number = RadarPreprocessing(self.data).radar_target_preprocess()
                for i in range(0, number):
                    target = RadarTargetPreprocessing(radar_target_precess[i])  # 将雷达数据中所有目标实例化
                    collect_radar_data.append(target)
            elif len(collect_radar_data) == 19:
                # 采集队列第一次被填满，进行目标的选择判断
                number = RadarPreprocessing(self.data).radar_target_preprocess()
                for i in range(0, number):
                    target = RadarTargetPreprocessing(radar_target_precess[i])  # 将雷达数据中所有目标实例化
                    collect_radar_data.append(target)
                # 做二十个目标的选取计算，得到最确信的目标
                count = []
                for m in range(len(collect_radar_data)):
                    dis = []
                    for n in range(len(collect_radar_data)):
                        distance = math.sqrt(
                            math.pow(float(collect_radar_data[m].x) - float(collect_radar_data[n].x), 2) +
                            math.pow(float(collect_radar_data[m].y) - float(collect_radar_data[n].y), 2))
                        dis.append(distance)
                    value = sum(d < 1 for d in dis)
                    count.append(value)
                # 找最确信目标
                if max(count) >= 6:
                    sure_target_index = count.index(max(count))
                    sure_target_deque.append(collect_radar_data[sure_target_index])
                    sure_target_possible.append(1)
                    filter_output.append(collect_radar_data[sure_target_index])
                    out_data = str(len(filter_output)) + ';'
                    for j in range(len(filter_output)):
                        out_data += filter_output[j].complete_data
                    mutex_radar.acquire()  # 加锁
                    radar_filter_communication.append(out_data)
                    mutex_radar.release()  # 解锁
                    radar_file = open('radar_filter_data.txt', mode='a')
                    radar_file.write(out_data + '\n')
                elif max(count) < 6:
                    collect_radar_data.popleft()
            elif len(collect_radar_data) == 20:
                # 非初始情况，将新到数据与确信队列、可能目标队列、采集队列进行比较
                number = RadarPreprocessing(self.data).radar_target_preprocess()
                miss_data_flag = 0
                for i in range(0, number):
                    self.target = RadarTargetPreprocessing(radar_target_precess[i])  # 将雷达数据中所有目标实例化
                    # 先与确信目标队列比较
                    for j in range(len(sure_target_deque)):
                        error_x = float(self.target.x) - float(sure_target_deque[j].x)
                        error_y = float(self.target.y) - float(sure_target_deque[j].y)
                        error = (error_x ** 2 + error_y ** 2) ** 0.5
                        if error < 1:
                            aver_sure_x = (2 * float(sure_target_deque[j].x) + float(self.target.x)) / 3
                            aver_sure_y = (2 * float(sure_target_deque[j].y) + float(self.target.y)) / 3
                            old_sure_target = AverRadarTargetPreprocessing(self.target.complete_data,
                                                                           aver_sure_x, aver_sure_y)
                            sure_target_deque[j] = old_sure_target
                            sure_target_possible[j] = sure_target_possible[j] + 1
                            filter_output.append(old_sure_target)
                            collect_radar_data.append(self.target)
                            self.get_flag = 1
                            break
                    if self.get_flag == 1:
                        self.get_flag = 0
                        continue
                    # 再与可能目标队列比较
                    for m in range(len(noise_target_deque)):
                        error_x = float(self.target.x) - float(noise_target_deque[m].x)
                        error_y = float(self.target.y) - float(noise_target_deque[m].y)
                        error = (error_x ** 2 + error_y ** 2) ** 0.5
                        if error < 1:
                            aver_sure_x = (2 * float(noise_target_deque[m].x) + float(self.target.x)) / 3
                            aver_sure_y = (2 * float(noise_target_deque[m].y) + float(self.target.y)) / 3
                            new_sure_target = AverRadarTargetPreprocessing(self.target.complete_data,
                                                                           aver_sure_x, aver_sure_y)
                            sure_target_deque.append(new_sure_target)
                            sure_target_possible.append(1)
                            noise_target_deque.__delitem__(m)
                            filter_output.append(new_sure_target)
                            collect_radar_data.append(self.target)
                            self.get_flag = 1
                            break
                    if self.get_flag == 1:
                        self.get_flag = 0
                        continue
                    # 再与采集队列比较
                    error_list = []  # 定义空列表，存放新数据与以往值的误差值
                    for n in range(len(collect_radar_data)):
                        error_x = float(self.target.x) - float(collect_radar_data[n].x)
                        error_y = float(self.target.y) - float(collect_radar_data[n].y)
                        error = (error_x ** 2 + error_y ** 2) ** 0.5
                        error_list.append(error)
                    small_error_number = 0
                    small_error_index = []
                    for k in range(len(error_list)):
                        if error_list[k] < 1:
                            small_error_number += 1
                            small_error_index.append(k)
                    if small_error_number >= 4:
                        sum_target_x = 0
                        sum_target_y = 0
                        for a in range(small_error_number):
                            target_index = small_error_index[a]
                            sum_target_x += float(collect_radar_data[target_index].x)
                            sum_target_y += float(collect_radar_data[target_index].y)
                        sum_target_x += float(self.target.x)
                        sum_target_y += float(self.target.y)
                        aver_target_x = sum_target_x / (small_error_number + 1)
                        aver_target_y = sum_target_y / (small_error_number + 1)
                        sure_target = AverRadarTargetPreprocessing(self.target.complete_data, aver_target_x,
                                                                   aver_target_y)
                        noise_target_deque.append(sure_target)  # 完成对新到数据的操作
                        collect_radar_data.append(self.target)
                    elif small_error_number < 4:
                        if miss_data_flag == 0:
                            miss_data_flag = 1
                            miss_data_number += 1
                            if miss_data_number % 4 == 0:
                                missout_data = 'No Radar Data'
                                queue_radar_target.put(missout_data)
                                radar_file = open('radar_filter_data.txt', mode='a')
                                radar_file.write(missout_data + '\n')
                        collect_radar_data.append(self.target)
                if len(filter_output) > 0:
                    if len(filter_output) == len(sure_target_deque):
                        out_data = str(len(filter_output)) + ';'
                        for j in range(len(filter_output)):
                            out_data += filter_output[j].complete_data
                        mutex_radar.acquire()  # 加锁
                        radar_filter_communication.append(out_data)
                        mutex_radar.release()  # 解锁
                        radar_file = open('radar_filter_data.txt', mode='a')
                        radar_file.write(out_data + '\n')
                    elif len(filter_output) < len(sure_target_deque):
                        filter_length = len(filter_output)
                        for m in range(len(sure_target_deque)):
                            sure_error_list = []
                            for n in range(filter_length):
                                error_x = float(sure_target_deque[m].x) - float(filter_output[n].x)
                                error_y = float(sure_target_deque[m].y) - float(filter_output[n].y)
                                error = (error_x ** 2 + error_y ** 2) ** 0.5
                                sure_error_list.append(error)
                            if min(sure_error_list) > 1:
                                current_time = self.target.time
                                sure_target_time = sure_target_deque[m].time
                                target_possible = sure_target_possible[m]
                                if current_time - sure_target_time < 10 or target_possible >= 30:
                                    old_x = sure_target_deque[m].x
                                    old_y = sure_target_deque[m].y
                                    disappear_target = AverRadarTargetPreprocessing(self.target.complete_data, old_x,
                                                                                    old_y)
                                    filter_output.append(disappear_target)
                            if len(filter_output) == len(sure_target_deque):
                                break
                        out_data = str(len(filter_output)) + ';'
                        for j in range(len(filter_output)):
                            out_data += filter_output[j].complete_data
                        mutex_radar.acquire()  # 加锁
                        radar_filter_communication.append(out_data)
                        mutex_radar.release()  # 解锁
                        radar_file = open('radar_filter_data.txt', mode='a')
                        radar_file.write(out_data + '\n')

    # 声源数据滤波
    def sound_filter(self):
        open("sound_filter_data.txt", 'w').close()
        sound_aver_data.clear()
        while self.laser_filter_thread_flag is True:
            self.laser_flag = 0
            try:
                if self.laser_close_flag is True:
                    self.sound_data = sound_communication.get(timeout=0.5)
                elif self.laser_close_flag is False:
                    self.sound_data = sound_communication.get(timeout=1.5)
            except queue.Empty:
                if self.laser_close_flag is True:
                    out_data = 'No Sound Data'
                    queue_laser_target.put(out_data)
                    sound_file = open('sound_filter_data.txt', mode='a')
                    sound_file.write(out_data + '\n')
                    self.laser_flag = 1
                    continue
                elif self.laser_close_flag is False:
                    current_time = system_time()
                    last_sound_time = 0
                    if len(sound_aver_data) > 0:
                        last_sound_time = sound_aver_data[-1].time
                    if current_time - last_sound_time < 10 and len(sound_aver_data) > 0:
                        for i in range(3):
                            out_data = sound_aver_data[-1].complete_data + ';'
                            queue_laser_target.put(out_data)
                            sound_file = open('sound_filter_data.txt', mode='a')
                            sound_file.write(out_data + '\n')
                        self.laser_flag = 1
                        continue
                    elif current_time - last_sound_time >= 10:
                        for i in range(3):
                            out_data = 'No Sound Data'
                            queue_laser_target.put(out_data)
                            sound_file = open('sound_filter_data.txt', mode='a')
                            sound_file.write(out_data + '\n')
                        self.laser_flag = 1
                        continue
            if self.laser_flag == 1:
                continue
            number = SoundPreprocessing(self.sound_data).sound_target_preprocess()
            self.sound_target = SoundTargetPreprocessing(sound_target_precess[0])
            sound_aver_data.append(self.sound_target)
            self.out_sound_target = self.sound_target

            # small_error_list = []
            # for i in range(len(sound_aver_data)):
            #     error_x = float(self.sound_target.x) - float(sound_aver_data[i].x)
            #     error_y = float(self.sound_target.y) - float(sound_aver_data[i].y)
            #     error = (error_x ** 2 + error_y ** 2) ** 0.5
            #     if error < 1.0:
            #         small_error_list.append(error)
            # if len(small_error_list) >= len(sound_aver_data) / 2.0:
            #     self.out_sound_target = self.sound_target
            # elif len(small_error_list) < len(sound_aver_data) / 2.0:
            #     new_target_x = float(sound_aver_data[-1].x)
            #     new_target_y = float(sound_aver_data[-1].y)
            #     self.out_sound_target = AverSoundTargetPreprocessing(self.sound_target.complete_data, new_target_x, new_target_y)

            sound_out_data = '1;' + self.out_sound_target.complete_data

            # msg = '---------------------------------------' + '\n'
            # msg = msg + '(' + format(float(self.out_sound_target.x), '.3f') + ',' + format(float(self.out_sound_target.y), '.3f') + ')' + '\n'
            # self.signal_write_laser_msg.emit(msg)  # 激光浏览框显示
            # object_list = []
            # object_point = (float(self.out_sound_target.x), float(self.out_sound_target.y))
            # object_list.append(object_point)
            # self.signal_laser_flush.emit(object_list)  # 画布画出激光点
            # self.history_fusion_point(object_list, self.laser_history_point_list)

            mutex_sound.acquire()  # 加锁
            sound_filter_communication.append(sound_out_data)
            mutex_sound.release()  # 解锁
            sound_file = open('sound_filter_data.txt', mode='a')
            sound_file.write(sound_out_data + '\n')

    # 雷达时间配准
    def radar_thread(self):
        self.radar_flag = 0  # 初始化雷达更新标志，0表示未更新，1表示有更新，初始为0
        radar_sync_time_queue.append(self.radar_init_time)
        open("radar_synced_data.txt", 'w').close()

        while self.radar_time_thread_flag is True:
            # 加锁
            mutex_radar.acquire()
            radar_communication_copy = radar_filter_communication.copy()
            mutex_radar.release()
            # 解锁
            # 初始状态
            if len(radar_communication_copy) == 0:  # 通信队列长度为0，表示为初始状态，暂时还没接收到数据
                self.radar_flag = 0  # 雷达数据未更新
                print("雷达数据未更新")
                time.sleep(0.1)
                continue
            # 雷达刚更新+非初始状态
            elif len(radar_communication_copy) > 0:  # 通信队列长度大于1，说明为非初始状态或是雷达刚更新的初始状态
                newest_data = radar_communication_copy[-1]  # 获取雷达的最新数据
                newest_time = RadarPreprocessing(newest_data).time  # 提取最新帧的时间
                new_sync_time = radar_sync_time_queue[-1]  # 提取同步时间队列中最新的同步时间
                # 雷达刚更新的初始状态
                if len(radar_update) == 0:  # 更新雷达队列为0，表示为雷达刚更新的初始状态
                    first_radar_data = radar_communication_copy[0]  # 这个时间作为第二帧同步时间
                    first_radar_time = RadarPreprocessing(first_radar_data).time  # 提取最新帧的时间
                    first_time_diff = (first_radar_time - new_sync_time) % self.sync_averT  # 得到首帧与初始时间之间的时间差
                    first_sync_time = first_radar_time - first_time_diff  # 找到第二帧同步时间的位置
                    radar_sync_time_queue.append(first_sync_time)  # 更新同步时间队列
                    if len(radar_communication_copy) > 1:  # 表示雷达第一次同时收到了不止1帧的数据
                        for i in range(0, len(radar_communication_copy) - 1):  # 将通信队列中除最后一帧外其他帧放入异常队列中
                            abnormal_data = RadarDataAssociation(radar_communication_copy[i]).data_association()
                            unregistered_radar.append(abnormal_data)
                    radar_update.append(newest_data)  # 更新雷达的雷达更新队列，将最新帧放入
                # 非初始状态
                elif len(radar_update) > 0:  # 更新雷达队列长度大于0，表示非初始状态
                    # 雷达无更新
                    if newest_data == radar_update[-1]:  # 通信队列队尾与雷达更新队列队尾相同，表示雷达数据无更新
                        self.radar_flag = 0  # 雷达数据未更新
                        time.sleep(0.05)
                        continue
                    # 雷达有更新
                    elif newest_data != radar_update[-1]:  # 通信队列队尾与雷达更新队列队尾不同，表示雷达数据更新
                        if radar_communication_copy[-2] != radar_update[-1]:  # 雷达通信队列一次更新了多帧，对异常情况做处理
                            try:
                                position = radar_communication_copy.index(radar_update[-1])  # 找出上次队尾现在处于通信队列的位置
                                for i in range(position + 1,
                                               len(radar_communication_copy) - 1):  # 将通信队列中除最后一帧外其他帧放入异常队列中
                                    abnormal_data = RadarDataAssociation(
                                        radar_communication_copy[i]).data_association()
                                    unregistered_radar.append(abnormal_data)
                            except:
                                for i in range(len(radar_communication_copy) - 1):  # 将通信队列中除最后一帧外其他帧放入异常队列中
                                    abnormal_data = RadarDataAssociation(
                                        radar_communication_copy[i]).data_association()
                                    unregistered_radar.append(abnormal_data)
                        radar_update.append(newest_data)  # 更新雷达的雷达更新队列，将最新帧放入
                        self.radar_flag = 1  # 雷达数据有更新
                        diff_time_diff = (newest_time - new_sync_time) / self.radar_averT
                        if diff_time_diff > 10:
                            last_time_diff = (newest_time - new_sync_time) % self.sync_averT
                            last_sync_time = newest_time - last_time_diff
                            radar_sync_time_queue.append(last_sync_time)  # 更新同步时间队列
                        new_sync_time = radar_sync_time_queue[-1]  # 提取同步时间队列中最新的同步时间
                        cur_time_diff = newest_time - new_sync_time  # 得到最新帧与最新同步时间之间的时间差
                        number = int(cur_time_diff / self.sync_averT)  # 得到需要做时间配准的次数
                        for i in range(0, number):
                            sync_time = SyncTime(self.sync_averT).radar__sync__time()  # 得到最新的同步时间
                            RadarRegistration(self.radar_flag, sync_time,
                                              radar_communication_copy).radar_registration()  # 对同步时间做一次时间配准
                            self.radar_flag = 0  # 需要多次配准时只需要第一次将数据放入未配准队列里
                            print("雷达配准队列为", radar_data_queue)
                            queue_radar_target.put(radar_data_queue[-1])
                            radar_file = open('radar_synced_data.txt', mode='a')
                            radar_file.write(radar_data_queue[-1] + '\n')

    # 激光时间配准
    def sound_thread(self):
        # 初始化
        self.sound_flag = 0  # 声源更新标志，0表示未更新，1表示有更新，初始为0
        sound_sync_time_queue.append(self.sound_init_time)
        open("sound_synced_data.txt", 'w').close()

        while self.laser_time_thread_flag is True:
            # 加锁
            mutex_sound.acquire()
            sound_communication_copy = sound_filter_communication.copy()
            mutex_sound.release()
            # 解锁
            # 初始状态
            if len(sound_communication_copy) == 0:  # 通信队列长度为0，表示为初始状态，暂时还没接收到数据
                self.sound_flag = 0  # 声源数据未更新
                print("声源数据未更新")
                time.sleep(0.5)
                continue  # 无动作
            # 声源刚更新+非初始状态
            elif len(sound_communication_copy) > 0:  # 通信队列长度大于1，说明为非初始状态或是声源刚更新的初始状态
                newest_data = sound_communication_copy[-1]  # 获取声源的最新数据
                newest_time = SoundPreprocessing(newest_data).time  # 提取最新帧的时间
                new_sync_time = sound_sync_time_queue[-1]  # 提取同步时间队列中最新的同步时间
                # 声源刚更新的初始状态
                if len(sound_update) == 0:  # 更新声源队列为0，表示为声源刚更新的初始状态
                    first_sound_data = sound_communication_copy[0]  # 这个时间作为第二帧同步时间
                    first_sound_time = SoundPreprocessing(first_sound_data).time  # 提取最新帧的时间
                    first_time_diff = (first_sound_time - new_sync_time) % self.sync_averT  # 得到首帧与初始时间之间的时间差
                    first_sync_time = first_sound_time - first_time_diff  # 找到第二帧同步时间的位置
                    first_number_diff = int((first_sound_time - new_sync_time) / self.sync_averT)
                    sound_sync_time_queue.append(first_sync_time)  # 更新同步时间队列
                    new_sync_time = sound_sync_time_queue[-1]  # 提取同步时间队列中最新的同步时间
                    # 声源首帧来到后，进行第一次时间配准
                    if len(sound_communication_copy) > 1:  # 表示声源第一次同时收到了不止1帧的数据
                        for i in range(0, len(sound_communication_copy) - 1):  # 将通信队列中除最后一帧外其他帧放入异常队列中
                            abnormal_data = SoundDataAssociation(sound_communication_copy[i]).data_association()
                            unregistered_sound.append(abnormal_data)
                    sound_update.append(newest_data)  # 更新声源的声源更新队列，将最新帧放入
                # 非初始状态
                elif len(sound_update) > 0:  # 更新声源队列长度大于0，表示非初始状态
                    # 声源无更新
                    if newest_data == sound_update[-1]:  # 通信队列队尾与声源更新队列队尾相同，表示声源数据无更新
                        self.sound_flag = 0  # 声源数据未更新
                        print("声源数据无更新")
                        time.sleep(0.5)
                        continue
                    # 声源有更新
                    elif newest_data != sound_update[-1]:  # 通信队列队尾与声源更新队列队尾不同，表示声源数据更新
                        if sound_communication_copy[-2] != sound_update[-1]:  # 声源通信队列一次更新了多帧，对异常情况做处理
                            try:
                                position = sound_communication_copy.index(sound_update[-1])  # 找出上次队尾现在处于通信队列的位置
                                for i in range(position + 1,
                                               len(sound_communication_copy) - 1):  # 将通信队列中除最后一帧外其他帧放入异常队列中
                                    abnormal_data = SoundDataAssociation(
                                        sound_communication_copy[i]).data_association()
                                    unregistered_sound.append(abnormal_data)
                            except:
                                for i in range(len(sound_communication_copy) - 1):  # 将通信队列中除最后一帧外其他帧放入异常队列中
                                    abnormal_data = SoundDataAssociation(
                                        sound_communication_copy[i]).data_association()
                                    unregistered_sound.append(abnormal_data)
                        sound_update.append(newest_data)  # 更新声源的声源更新队列，将最新帧放入
                        self.sound_flag = 1  # 声源数据有更新
                        time_diff = (newest_time - new_sync_time) / self.sound_averT
                        if time_diff > 3:
                            last_time_diff = (newest_time - new_sync_time) % self.sync_averT
                            last_sync_time = newest_time - last_time_diff
                            last_number_diff = int((newest_time - new_sync_time) / self.sync_averT)
                            sound_sync_time_queue.append(last_sync_time)  # 更新同步时间队列
                        new_sync_time = sound_sync_time_queue[-1]  # 提取同步时间队列中最新的同步时间
                        time_diff = newest_time - new_sync_time  # 得到最新帧与最新同步时间之间的时间差
                        number = int(time_diff / self.sync_averT)  # 得到需要做时间配准的次数
                        for i in range(0, number):
                            sync_time = SyncTime(self.sync_averT).sound__sync__time()  # 得到最新的同步时间
                            SoundRegistration(self.sound_flag, sync_time,
                                              sound_communication_copy).sound_registration()  # 对同步时间做一次时间配准
                            self.sound_flag = 0  # 需要多次配准时只需要第一次将数据放入未配准队列里
                            #  打印保存信息
                            print("声源配准队列为", sound_data_queue)
                            queue_laser_target.put(sound_data_queue[-1])
                            sound_file = open('sound_synced_data.txt', mode='a')
                            sound_file.write(sound_data_queue[-1] + '\n')

    # 单目标数据融合左
    def single_queue_fusion0(self):
        open("radar_fusion_data.txt", 'w').close()
        open("sound_fusion_data.txt", 'w').close()
        while self.fusion_thread_flag is True:
            radar_x = []
            radar_y = []
            laser_x = []
            laser_y = []
            fusion_list = []
            msg = ''
            radar = queue_radar_target.get()
            laser = queue_laser_target.get()
            print('已收到融合数据')
            radar_file = open('radar_fusion_data.txt', mode='a')
            radar_file.write(radar + '\n')
            sound_file = open('sound_fusion_data.txt', mode='a')
            sound_file.write(laser + '\n')

            sure_radar = ''
            if radar == 'No Radar Data' and laser != 'No Sound Data' and len(sure_target_deque) > 0:
                sure_laser = re.split(r',', laser)
                laser_time = sure_laser[0] + ',' + sure_laser[1] + ',' + sure_laser[2] + ',' + sure_laser[3] + ',' \
                             + sure_laser[4] + ',' + sure_laser[5] + ',' + sure_laser[6] + ','
                sure_radar += laser_time
                for i in range(len(sure_target_deque)):
                    sure_radar += sure_target_deque[i].sure_data
                radar = sure_radar

            if radar == 'No Radar Data' and laser == 'No Sound Data':
                # 两个都无数据
                self.radar_fusion_time = 'No Radar Data'
                self.laser_fusion_time = 'No Sound Data'
                print('融合数据都为空')
                time.sleep(0.1)
                continue

            elif radar != 'No Radar Data' and laser == 'No Sound Data':
                # 雷达有数据，声源无数据
                self.radar_fusion_time = radar
                self.laser_fusion_time = 'No Sound Data'
                corn = (self.corner_x, self.corner_y)
                radar_split_data = re.split(r';', radar)
                for i in range(len(radar_split_data) - 1):
                    radar_target_data = re.split(r',', radar_split_data[i])
                    radar_x.append(float(radar_target_data[-2]))
                    radar_y.append(float(radar_target_data[-1]))
                for m in range(len(radar_x)):
                    if radar_x[m] >= corn[0] + 4:
                        target = (radar_x[m], radar_y[m])
                        fusion_list.append(target)
                print("单雷达融合后各目标数据为", radar)

            elif radar == 'No Radar Data' and laser != 'No Sound Data':
                # 雷达无数据，声源有数据
                if self.single_fusion_flag is False:
                    self.radar_fusion_time = 'No Radar Data'
                    self.laser_fusion_time = laser
                    laser_split_data = re.split(r';', laser)
                    laser_target_data = re.split(r',', laser_split_data[0])
                    laser_x = float(laser_target_data[-3])
                    laser_y = float(laser_target_data[-2])
                    target = (laser_x, laser_y)
                    fusion_list.append(target)
                    print("单声源融合后目标数据为", laser)

                elif self.single_fusion_flag is True:
                    print("单声源融合去除声源点的目标数据为", laser)
                    continue

            elif radar != 'No Radar Data' and laser != 'No Sound Data':
                radar_data = re.split(r',', radar)
                laser_data = re.split(r',', laser)
                self.radar_fusion_time = radar
                radar_time = radar_data[0] + ',' + radar_data[1] + ',' + radar_data[2] + ',' + radar_data[3] + ',' \
                             + radar_data[4] + ',' + radar_data[5] + ',' + radar_data[6] + ','
                self.laser_fusion_time = radar_time + laser_data[-4] + ',' + laser_data[-3] + ',' \
                                         + laser_data[-2] + ',' + laser_data[-1] + ';'
                radar_split_data = re.split(r';', radar)
                laser_split_data = re.split(r';', laser)
                for i in range(len(radar_split_data) - 1):
                    radar_target_data = re.split(r',', radar_split_data[i])
                    radar_x.append(float(radar_target_data[-2]))
                    radar_y.append(float(radar_target_data[-1]))
                for j in range(len(laser_split_data) - 1):
                    laser_target_data = re.split(r',', laser_split_data[j])
                    laser_x.append(float(laser_target_data[-3]))
                    laser_y.append(float(laser_target_data[-2]))

                corn = (self.corner_x, self.corner_y)
                true_target = [(5.4, 2.74), (5.4, 2.13), (5.4, 1.47), (7.16, 2.72), (7.56, 2.0), (10.17, 8.04),
                               (10.02, 2.47)]
                for m in range(len(true_target)):
                    for n in range(len(radar_x)):
                        error_x = radar_x[n] - true_target[m][0] - corn[0]
                        error_y = radar_y[n] - true_target[m][1] - corn[1]
                        error = (error_x ** 2 + error_y ** 2) ** 0.5
                        if error < 0.75:
                            # 说明雷达监测目标为某个真实目标附近
                            self.single_fusion_number += 1
                if self.single_fusion_number >= 5:
                    self.single_fusion_flag = True
                if self.single_fusion_flag is False:
                    # 构建关联矩阵，行为声源目标，列为雷达目标
                    row = len(radar_x)  # 关联矩阵的行数——雷达的目标数量 3
                    col = len(laser_x)  # 关联矩阵的列数——声源的目标数量 1
                    association_matrix = np.zeros(shape=(row, col))  # 建立空的关联矩阵
                    dis_radar = []
                    for i in range(row):
                        dis = 0
                        for j in range(col):
                            error_x = abs(radar_x[i] - laser_x[j])
                            error_y = abs(radar_y[i] - laser_y[j])
                            error = (error_x ** 2 + error_y ** 2) ** 0.5
                            association_matrix[i][j] = error  # 将误差放入关联矩阵中
                            dis += error
                        dis_radar.append(dis)
                    min_dis_radar_id = dis_radar.index(min(dis_radar))
                    dis_laser = []
                    for k in range(col):
                        dis_laser.append(association_matrix[min_dis_radar_id][k])
                    min_dis_laser_id = dis_laser.index(min(dis_laser))
                    radar_laser_min_error = association_matrix[min_dis_radar_id][min_dis_laser_id]
                    if radar_laser_min_error > 2:
                        laser_target_x = sum(laser_x) / len(laser_x)
                        laser_target_y = sum(laser_y) / len(laser_y)
                        laser_target = (laser_target_x, laser_target_y)
                        laser_radar_dis = []
                        if len(noise_target_deque) > 0:
                            for i in range(len(noise_target_deque)):
                                error_x = laser_target_x - float(noise_target_deque[i].x)
                                error_y = laser_target_y - float(noise_target_deque[i].y)
                                error = (error_x ** 2 + error_y ** 2) ** 0.5
                                laser_radar_dis.append(error)
                            min_dis_error = min(laser_radar_dis)
                            if min_dis_error <= 1:
                                fusion_list.append(laser_target)
                                print("声源点可能是单独的目标，其位置为(%s,%s)" % (laser_target_x, laser_target_y))
                    elif radar_laser_min_error <= 2:
                        print("此时可能是位置为(%s,%s)的雷达目标在说话" % (radar_x[min_dis_radar_id], radar_y[min_dis_radar_id]))
                        print("声源点与雷达相近，其坐标点可能为(%s,%s)" % (laser_x[min_dis_laser_id], laser_y[min_dis_laser_id]))
                        if radar_x[min_dis_radar_id] >= corn[0] and radar_x[min_dis_radar_id] < corn[0] + 4:
                            fusion_target_x = (0 * radar_x.pop(min_dis_radar_id) + 1 * laser_x.pop(
                                min_dis_laser_id)) / 1
                            fusion_target_y = (0 * radar_y.pop(min_dis_radar_id) + 1 * laser_y.pop(
                                min_dis_laser_id)) / 1
                            fusion_target = (fusion_target_x, fusion_target_y)
                            fusion_list.append(fusion_target)
                        elif radar_x[min_dis_radar_id] >= corn[0] + 4 and radar_x[min_dis_radar_id] < corn[0] + 13:
                            fusion_target_x = (radar_x.pop(min_dis_radar_id) + 0 * laser_x.pop(min_dis_laser_id)) / 1
                            fusion_target_y = (radar_y.pop(min_dis_radar_id) + 0 * laser_y.pop(min_dis_laser_id)) / 1
                            fusion_target = (fusion_target_x, fusion_target_y)
                            fusion_list.append(fusion_target)
                    for m in range(len(radar_x)):
                        target = (radar_x[m], radar_y[m])
                        fusion_list.append(target)
                    print("此次融合的雷达数据为%s，声源数据为%s" % (radar, laser))
                    print("全部融合后各目标位置为", fusion_list)

                elif self.single_fusion_flag is True:
                    # 雷达点在3,4,5,6,7,8,9上，不做融合，直接用雷达点
                    for m in range(len(radar_x)):
                        target = (radar_x[m], radar_y[m])
                        fusion_list.append(target)
                    print("去除声源与雷达融合后各目标数据为", radar)

            pop_index = []
            for m in range(len(fusion_list)):
                for n in range(m + 1, len(fusion_list)):
                    error_x = abs(fusion_list[m][0] - fusion_list[n][0])
                    error_y = abs(fusion_list[m][1] - fusion_list[n][1])
                    error = (error_x ** 2 + error_y ** 2) ** 0.5
                    if error < 0.5:
                        pop_index.append(m)
            for j in range(len(pop_index)):
                del fusion_list[pop_index[j]]

            for i in range(len(fusion_list)):
                object_radar_x = format(fusion_list[i][0], '.3f')
                object_radar_y = format(fusion_list[i][1], '.3f')
                msg = msg + '(' + str(object_radar_x) + ',' + str(object_radar_y) + ')'
            msg = msg + '\n'

            self.fusion_object_list_ui = fusion_list
            self.signal_fusion_flush.emit(fusion_list)
            # self.history_fusion_point(fusion_list, self.fusion_history_point_list)
            self.signal_write_fusion_msg.emit('--------------------------------------\n')
            self.signal_write_fusion_msg.emit(msg)

    # 单目标数据融合中
    def single_queue_fusion1(self):
        open("radar_fusion_data.txt", 'w').close()
        open("sound_fusion_data.txt", 'w').close()
        while self.fusion_thread_flag is True:
            radar_x = []
            radar_y = []
            laser_x = []
            laser_y = []
            fusion_list = []
            msg = ''
            radar = queue_radar_target.get()
            laser = queue_laser_target.get()
            print('已收到融合数据')
            radar_file = open('radar_fusion_data.txt', mode='a')
            radar_file.write(radar + '\n')
            sound_file = open('sound_fusion_data.txt', mode='a')
            sound_file.write(laser + '\n')

            sure_radar = ''
            if radar == 'No Radar Data' and laser != 'No Sound Data' and len(sure_target_deque) > 0:
                sure_laser = re.split(r',', laser)
                laser_time = sure_laser[0] + ',' + sure_laser[1] + ',' + sure_laser[2] + ',' + sure_laser[3] + ',' \
                             + sure_laser[4] + ',' + sure_laser[5] + ',' + sure_laser[6] + ','
                sure_radar += laser_time
                for i in range(len(sure_target_deque)):
                    sure_radar += sure_target_deque[i].sure_data
                radar = sure_radar

            if radar == 'No Radar Data' and laser == 'No Sound Data':
                # 两个都无数据
                self.radar_fusion_time = 'No Radar Data'
                self.laser_fusion_time = 'No Sound Data'
                print('融合数据都为空')
                time.sleep(0.1)
                continue

            elif radar != 'No Radar Data' and laser == 'No Sound Data':
                # 雷达有数据，声源无数据
                self.radar_fusion_time = radar
                self.laser_fusion_time = 'No Sound Data'
                radar_split_data = re.split(r';', radar)
                for i in range(len(radar_split_data) - 1):
                    radar_target_data = re.split(r',', radar_split_data[i])
                    radar_x.append(float(radar_target_data[-2]))
                    radar_y.append(float(radar_target_data[-1]))
                for m in range(len(radar_x)):
                    target = (radar_x[m], radar_y[m])
                    fusion_list.append(target)
                print("单雷达融合后各目标数据为", radar)

            elif radar == 'No Radar Data' and laser != 'No Sound Data':
                # 雷达无数据，声源有数据
                if self.single_fusion_flag is False:
                    self.radar_fusion_time = 'No Radar Data'
                    self.laser_fusion_time = laser
                    laser_split_data = re.split(r';', laser)
                    laser_target_data = re.split(r',', laser_split_data[0])
                    laser_x = float(laser_target_data[-3])
                    laser_y = float(laser_target_data[-2])
                    target = (laser_x, laser_y)
                    fusion_list.append(target)
                    print("单声源融合后目标数据为", laser)

                elif self.single_fusion_flag is True:
                    print("单声源融合去除声源点的目标数据为", laser)
                    continue

            elif radar != 'No Radar Data' and laser != 'No Sound Data':
                radar_data = re.split(r',', radar)
                laser_data = re.split(r',', laser)
                self.radar_fusion_time = radar
                radar_time = radar_data[0] + ',' + radar_data[1] + ',' + radar_data[2] + ',' + radar_data[3] + ',' \
                             + radar_data[4] + ',' + radar_data[5] + ',' + radar_data[6] + ','
                self.laser_fusion_time = radar_time + laser_data[-4] + ',' + laser_data[-3] + ',' \
                                         + laser_data[-2] + ',' + laser_data[-1] + ';'
                radar_split_data = re.split(r';', radar)
                laser_split_data = re.split(r';', laser)
                for i in range(len(radar_split_data) - 1):
                    radar_target_data = re.split(r',', radar_split_data[i])
                    radar_x.append(float(radar_target_data[-2]))
                    radar_y.append(float(radar_target_data[-1]))
                for j in range(len(laser_split_data) - 1):
                    laser_target_data = re.split(r',', laser_split_data[j])
                    laser_x.append(float(laser_target_data[-3]))
                    laser_y.append(float(laser_target_data[-2]))

                corn = (self.corner_x, self.corner_y)
                true_target = [(0.48, 1.57), (2.47, 1.38), (7.16, 2.72), (7.56, 2.0), (10.17, 8.04), (10.02, 2.47)]
                for m in range(len(true_target)):
                    for n in range(len(radar_x)):
                        error_x = radar_x[n] - true_target[m][0] - corn[0]
                        error_y = radar_y[n] - true_target[m][1] - corn[1]
                        error = (error_x ** 2 + error_y ** 2) ** 0.5
                        if error < 0.75:
                            # 说明雷达监测目标为某个真实目标附近
                            self.single_fusion_number += 1
                if self.single_fusion_number >= 5:
                    self.single_fusion_flag = True
                if self.single_fusion_flag is False:
                    # 构建关联矩阵，行为声源目标，列为雷达目标
                    row = len(radar_x)  # 关联矩阵的行数——雷达的目标数量 3
                    col = len(laser_x)  # 关联矩阵的列数——声源的目标数量 1
                    association_matrix = np.zeros(shape=(row, col))  # 建立空的关联矩阵
                    dis_radar = []
                    for i in range(row):
                        dis = 0
                        for j in range(col):
                            error_x = abs(radar_x[i] - laser_x[j])
                            error_y = abs(radar_y[i] - laser_y[j])
                            error = (error_x ** 2 + error_y ** 2) ** 0.5
                            association_matrix[i][j] = error  # 将误差放入关联矩阵中
                            dis += error
                        dis_radar.append(dis)
                    min_dis_radar_id = dis_radar.index(min(dis_radar))
                    dis_laser = []
                    for k in range(col):
                        dis_laser.append(association_matrix[min_dis_radar_id][k])
                    min_dis_laser_id = dis_laser.index(min(dis_laser))
                    radar_laser_min_error = association_matrix[min_dis_radar_id][min_dis_laser_id]
                    if radar_laser_min_error > 2:
                        laser_target_x = sum(laser_x) / len(laser_x)
                        laser_target_y = sum(laser_y) / len(laser_y)
                        laser_target = (laser_target_x, laser_target_y)
                        laser_radar_dis = []
                        if len(noise_target_deque) > 0:
                            for i in range(len(noise_target_deque)):
                                error_x = laser_target_x - float(noise_target_deque[i].x)
                                error_y = laser_target_y - float(noise_target_deque[i].y)
                                error = (error_x ** 2 + error_y ** 2) ** 0.5
                                laser_radar_dis.append(error)
                            min_dis_error = min(laser_radar_dis)
                            if min_dis_error <= 1:
                                fusion_list.append(laser_target)
                                print("声源点可能是单独的目标，其位置为(%s,%s)" % (laser_target_x, laser_target_y))
                    elif radar_laser_min_error <= 2:
                        print("此时可能是位置为(%s,%s)的雷达目标在说话" % (radar_x[min_dis_radar_id], radar_y[min_dis_radar_id]))
                        print("声源点与雷达相近，其坐标点可能为(%s,%s)" % (laser_x[min_dis_laser_id], laser_y[min_dis_laser_id]))
                        if radar_x[min_dis_radar_id] >= corn[0] and radar_x[min_dis_radar_id] < corn[0] + 4:
                            fusion_target_x = (1 * radar_x.pop(min_dis_radar_id) + 0 * laser_x.pop(
                                min_dis_laser_id)) / 1
                            fusion_target_y = (1 * radar_y.pop(min_dis_radar_id) + 0 * laser_y.pop(
                                min_dis_laser_id)) / 1
                            fusion_target = (fusion_target_x, fusion_target_y)
                            fusion_list.append(fusion_target)
                        elif radar_x[min_dis_radar_id] >= corn[0] + 4 and radar_x[min_dis_radar_id] < corn[0] + 8:
                            fusion_target_x = (radar_x.pop(min_dis_radar_id) + laser_x.pop(min_dis_laser_id)) / 2
                            fusion_target_y = (radar_y.pop(min_dis_radar_id) + laser_y.pop(min_dis_laser_id)) / 2
                            fusion_target = (fusion_target_x, fusion_target_y)
                            fusion_list.append(fusion_target)
                        elif radar_x[min_dis_radar_id] >= corn[0] + 8 and radar_x[min_dis_radar_id] < corn[0] + 13:
                            fusion_target_x = (4 * radar_x.pop(min_dis_radar_id) + 0 * laser_x.pop(
                                min_dis_laser_id)) / 4
                            fusion_target_y = (4 * radar_y.pop(min_dis_radar_id) + 0 * laser_y.pop(
                                min_dis_laser_id)) / 4
                            fusion_target = (fusion_target_x, fusion_target_y)
                            fusion_list.append(fusion_target)
                    for m in range(len(radar_x)):
                        target = (radar_x[m], radar_y[m])
                        fusion_list.append(target)
                    print("此次融合的雷达数据为%s，声源数据为%s" % (radar, laser))
                    print("全部融合后各目标位置为", fusion_list)

                elif self.single_fusion_flag is True:
                    # 雷达点在1,2,6,7,8,9上，不做融合，直接用雷达点
                    for m in range(len(radar_x)):
                        target = (radar_x[m], radar_y[m])
                        fusion_list.append(target)
                    print("去除声源与雷达融合后各目标数据为", radar)

            pop_index = []
            for m in range(len(fusion_list)):
                for n in range(m + 1, len(fusion_list)):
                    error_x = abs(fusion_list[m][0] - fusion_list[n][0])
                    error_y = abs(fusion_list[m][1] - fusion_list[n][1])
                    error = (error_x ** 2 + error_y ** 2) ** 0.5
                    if error < 0.5:
                        pop_index.append(m)
            for j in range(len(pop_index)):
                del fusion_list[pop_index[j]]

            for i in range(len(fusion_list)):
                object_radar_x = format(fusion_list[i][0], '.3f')
                object_radar_y = format(fusion_list[i][1], '.3f')
                msg = msg + '(' + str(object_radar_x) + ',' + str(object_radar_y) + ')'
            msg = msg + '\n'
            self.fusion_object_list_ui = fusion_list
            self.signal_fusion_flush.emit(fusion_list)
            self.history_fusion_point(fusion_list, self.fusion_history_point_list)
            self.signal_write_fusion_msg.emit('--------------------------------------\n')
            self.signal_write_fusion_msg.emit(msg)

    # 多目标数据融合
    def multi_queue_fusion(self):
        open("radar_fusion_data.txt", 'w').close()
        open("sound_fusion_data.txt", 'w').close()
        while self.fusion_thread_flag is True:
            radar_x = []
            radar_y = []
            laser_x = []
            laser_y = []
            msg = ''
            fusion_list = []
            radar = queue_radar_target.get()
            laser = queue_laser_target.get()
            print('已收到融合数据')
            radar_file = open('radar_fusion_data.txt', mode='a')
            radar_file.write(radar + '\n')
            sound_file = open('sound_fusion_data.txt', mode='a')
            sound_file.write(laser + '\n')

            sure_radar = ''
            if radar == 'No Radar Data' and laser != 'No Sound Data' and len(sure_target_deque) > 0:
                sure_laser = re.split(r',', laser)
                laser_time = sure_laser[0] + ',' + sure_laser[1] + ',' + sure_laser[2] + ',' + sure_laser[3] + ',' \
                             + sure_laser[4] + ',' + sure_laser[5] + ',' + sure_laser[6] + ','
                sure_radar += laser_time
                for i in range(len(sure_target_deque)):
                    sure_radar += sure_target_deque[i].sure_data
                radar = sure_radar

            if radar == 'No Radar Data' and laser == 'No Sound Data':
                # 两个都无数据
                self.radar_fusion_time = 'No Radar Data'
                self.laser_fusion_time = 'No Sound Data'
                print('融合数据都为空')
                time.sleep(0.1)
                continue

            elif radar != 'No Radar Data' and laser == 'No Sound Data':
                # 雷达有数据，声源无数据
                self.radar_fusion_time = radar
                self.laser_fusion_time = 'No Sound Data'
                radar_split_data = re.split(r';', radar)
                for i in range(len(radar_split_data) - 1):
                    radar_target_data = re.split(r',', radar_split_data[i])
                    radar_x.append(float(radar_target_data[-2]))
                    radar_y.append(float(radar_target_data[-1]))
                for m in range(len(radar_x)):
                    target = (radar_x[m], radar_y[m])
                    fusion_list.append(target)
                print("单雷达融合后各目标数据为", radar)

            elif radar == 'No Radar Data' and laser != 'No Sound Data':
                # 雷达无数据，声源有数据
                self.radar_fusion_time = 'No Radar Data'
                self.laser_fusion_time = laser
                laser_split_data = re.split(r';', laser)
                laser_target_data = re.split(r',', laser_split_data[0])
                laser_x = float(laser_target_data[-3])
                laser_y = float(laser_target_data[-2])
                target = (laser_x, laser_y)
                fusion_list.append(target)
                print("单声源融合后目标数据为", laser)

            elif radar != 'No Radar Data' and laser != 'No Sound Data':
                # radar_data = re.split(r',', radar)
                # radar_time = float(radar_data[0])
                # laser_data = re.split(r',', laser)
                # laser_time = float(laser_data[0])
                # if radar_time > laser_time:
                #     time_diff_num = (radar_time - laser_time) / self.sync_averT
                radar_data = re.split(r',', radar)
                laser_data = re.split(r',', laser)
                self.radar_fusion_time = radar
                radar_time = radar_data[0] + ',' + radar_data[1] + ',' + radar_data[2] + ',' + radar_data[3] + ',' \
                             + radar_data[4] + ',' + radar_data[5] + ',' + radar_data[6] + ','
                self.laser_fusion_time = radar_time + laser_data[-4] + ',' + laser_data[-3] + ',' \
                                         + laser_data[-2] + ',' + laser_data[-1] + ';'
                radar_split_data = re.split(r';', radar)
                laser_split_data = re.split(r';', laser)
                for i in range(len(radar_split_data) - 1):
                    radar_target_data = re.split(r',', radar_split_data[i])
                    radar_x.append(float(radar_target_data[-2]))
                    radar_y.append(float(radar_target_data[-1]))
                for j in range(len(laser_split_data) - 1):
                    laser_target_data = re.split(r',', laser_split_data[j])
                    laser_x.append(float(laser_target_data[-3]))
                    laser_y.append(float(laser_target_data[-2]))
                # 构建关联矩阵，行为声源目标，列为雷达目标
                row = len(radar_x)  # 关联矩阵的行数——雷达的目标数量 3
                col = len(laser_x)  # 关联矩阵的列数——声源的目标数量 1
                association_matrix = np.zeros(shape=(row, col))  # 建立空的关联矩阵
                dis_radar = []
                for i in range(row):
                    dis = 0
                    for j in range(col):
                        error_x = abs(radar_x[i] - laser_x[j])
                        error_y = abs(radar_y[i] - laser_y[j])
                        error = (error_x ** 2 + error_y ** 2) ** 0.5
                        association_matrix[i][j] = error  # 将误差放入关联矩阵中
                        dis += error
                    dis_radar.append(dis)
                min_dis_radar_id = dis_radar.index(min(dis_radar))
                dis_laser = []
                for k in range(col):
                    dis_laser.append(association_matrix[min_dis_radar_id][k])
                min_dis_laser_id = dis_laser.index(min(dis_laser))
                radar_laser_min_error = association_matrix[min_dis_radar_id][min_dis_laser_id]
                corn = (self.corner_x, self.corner_y)
                if radar_laser_min_error > 2:
                    laser_target_x = sum(laser_x) / len(laser_x)
                    laser_target_y = sum(laser_y) / len(laser_y)
                    laser_target = (laser_target_x, laser_target_y)
                    if laser_target[0] >= corn[0] and laser_target[0] <= corn[0] + 4:
                        fusion_list.append(laser_target)
                        print("声源点可能是单独的目标，其位置为(%s,%s)" % (laser_target_x, laser_target_y))
                elif radar_laser_min_error <= 2:
                    print("此时可能是位置为(%s,%s)的雷达目标在说话" % (radar_x[min_dis_radar_id], radar_y[min_dis_radar_id]))
                    print("声源点与雷达相近，其坐标点可能为(%s,%s)" % (laser_x[min_dis_laser_id], laser_y[min_dis_laser_id]))
                    if radar_x[min_dis_radar_id] >= corn[0] and radar_x[min_dis_radar_id] < corn[0] + 4:
                        fusion_target_x = (0 * radar_x.pop(min_dis_radar_id) + 1 * laser_x.pop(min_dis_laser_id)) / 1
                        fusion_target_y = (0 * radar_y.pop(min_dis_radar_id) + 1 * laser_y.pop(min_dis_laser_id)) / 1
                        fusion_target = (fusion_target_x, fusion_target_y)
                        fusion_list.append(fusion_target)
                    elif radar_x[min_dis_radar_id] >= corn[0] + 4 and radar_x[min_dis_radar_id] < corn[0] + 13:
                        fusion_target_x = (1 * radar_x.pop(min_dis_radar_id) + 0 * laser_x.pop(min_dis_laser_id)) / 1
                        fusion_target_y = (1 * radar_y.pop(min_dis_radar_id) + 0 * laser_y.pop(min_dis_laser_id)) / 1
                        fusion_target = (fusion_target_x, fusion_target_y)
                        fusion_list.append(fusion_target)
                for m in range(len(radar_x)):
                    if radar_x[m] >= corn[0] + 4:
                        target = (radar_x[m], radar_y[m])
                        fusion_list.append(target)
                print("此次融合的雷达数据为%s，声源数据为%s" % (radar, laser))
                print("全部融合后各目标位置为", fusion_list)

            pop_index = []
            for m in range(len(fusion_list)):
                for n in range(m + 1, len(fusion_list)):
                    error_x = abs(fusion_list[m][0] - fusion_list[n][0])
                    error_y = abs(fusion_list[m][1] - fusion_list[n][1])
                    error = (error_x ** 2 + error_y ** 2) ** 0.5
                    if error < 1:
                        pop_index.append(m)
            for j in range(len(pop_index)):
                del fusion_list[pop_index[j]]

            for i in range(len(fusion_list)):
                object_radar_x = format(fusion_list[i][0], '.3f')
                object_radar_y = format(fusion_list[i][1], '.3f')
                msg = msg + '(' + str(object_radar_x) + ',' + str(object_radar_y) + ')'
            msg = msg + '\n'
            self.fusion_object_list_ui = fusion_list
            self.signal_fusion_flush.emit(fusion_list)
            self.history_fusion_point(fusion_list, self.fusion_history_point_list)
            self.signal_write_fusion_msg.emit('--------------------------------------\n')
            self.signal_write_fusion_msg.emit(msg)

    # 数据的融合
    # def queue_fusion(self):
    #     while self.fusion_thread_flag is True:
    #         radar_x = []
    #         radar_y = []
    #         laser_x = []
    #         laser_y = []
    #         fusion_list = []
    #         radar = queue_radar_target.get()
    #         laser = queue_laser_target.get()
    #         msg = '---------------------------------------' + '\n'
    #         if radar == 'No Radar Data' and laser == 'No Sound Data':
    #             # 两个都无数据
    #             time.sleep(0.1)
    #             continue
    #
    #         elif radar != 'No Radar Data' and laser == 'No Sound Data':
    #             # 雷达有数据，声源无数据
    #             radar_split_data = re.split(r';', radar)
    #             for i in range(len(radar_split_data) - 1):
    #                 radar_target_data = re.split(r',', radar_split_data[i])
    #                 radar_x.append(float(radar_target_data[-2]))
    #                 radar_y.append(float(radar_target_data[-1]))
    #             for m in range(len(radar_x)):
    #                 msg = msg + '(' + format(radar_x[m], '.3f') + ',' + format(radar_y[m], '.3f') + ')'
    #                 target = (radar_x[m], radar_y[m])
    #                 fusion_list.append(target)
    #             print("单雷达融合后各目标数据为", radar)
    #
    #         elif radar == 'No Radar Data' and laser != 'No Sound Data':
    #             # 雷达无数据，声源有数据
    #             laser_split_data = re.split(r';', laser)
    #             for j in range(len(laser_split_data) - 1):
    #                 laser_target_data = re.split(r',', laser_split_data[j])
    #                 laser_x.append(float(laser_target_data[-3]))
    #                 laser_y.append(float(laser_target_data[-2]))
    #             target_x = sum(laser_x) / len(laser_x)
    #             target_y = sum(laser_y) / len(laser_y)
    #             msg = msg + '(' + format(target_x, '.3f') + ',' + format(target_y, '.3f') + ')'
    #             target = (target_x, target_y)
    #             fusion_list.append(target)
    #             print("单声源融合后各目标数据为", laser)
    #
    #         elif radar != 'No Radar Data' and laser != 'No Sound Data':
    #             # radar_data = re.split(r',', radar)
    #             # radar_time = float(radar_data[0])
    #             # laser_data = re.split(r',', laser)
    #             # laser_time = float(laser_data[0])
    #             # if radar_time > laser_time:
    #             #     time_diff_num = (radar_time - laser_time) / self.sync_averT
    #             radar_split_data = re.split(r';', radar)
    #             laser_split_data = re.split(r';', laser)
    #             for i in range(len(radar_split_data) - 1):
    #                 radar_target_data = re.split(r',', radar_split_data[i])
    #                 radar_x.append(float(radar_target_data[-2]))
    #                 radar_y.append(float(radar_target_data[-1]))
    #             for j in range(len(laser_split_data) - 1):
    #                 laser_target_data = re.split(r',', laser_split_data[j])
    #                 laser_x.append(float(laser_target_data[-3]))
    #                 laser_y.append(float(laser_target_data[-2]))
    #             # 构建关联矩阵，行为声源目标，列为雷达目标
    #             row = len(radar_x)  # 关联矩阵的行数——雷达的目标数量 3
    #             col = len(laser_x)  # 关联矩阵的列数——声源的目标数量 1
    #             association_matrix = np.zeros(shape=(row, col))  # 建立空的关联矩阵
    #             dis_radar = []
    #             for i in range(row):
    #                 dis = 0
    #                 for j in range(col):
    #                     error_x = abs(radar_x[i] - laser_x[j])
    #                     error_y = abs(radar_y[i] - laser_y[j])
    #                     error = (error_x ** 2 + error_y ** 2) ** 0.5
    #                     association_matrix[i][j] = error  # 将误差放入关联矩阵中
    #                     dis += error
    #                 dis_radar.append(dis)
    #             min_dis_radar_id = dis_radar.index(min(dis_radar))
    #             dis_laser = []
    #             for k in range(col):
    #                 dis_laser.append(association_matrix[min_dis_radar_id][k])
    #             min_dis_laser_id = dis_laser.index(min(dis_laser))
    #             radar_laser_min_error = association_matrix[min_dis_radar_id][min_dis_laser_id]
    #             if radar_laser_min_error > 2:
    #                 laser_target_x = sum(laser_x) / len(laser_x)
    #                 laser_target_y = sum(laser_y) / len(laser_y)
    #                 laser_target = (laser_target_x, laser_target_y)
    #                 laser_radar_dis = []
    #                 if len(noise_target_deque) > 0:
    #                     for i in range(len(noise_target_deque)):
    #                         error_x = laser_target_x - float(noise_target_deque[i].x)
    #                         error_y = laser_target_y - float(noise_target_deque[i].y)
    #                         error = (error_x ** 2 + error_y ** 2) ** 0.5
    #                         laser_radar_dis.append(error)
    #                     min_dis_error = min(laser_radar_dis)
    #                     if min_dis_error <= 1:
    #                         fusion_list.append(laser_target)
    #                         print("声源点可能是单独的目标，其位置为(%s,%s)" % (laser_target_x, laser_target_y))
    #                 elif len(noise_target_deque) == 0:
    #                     fusion_list.append(laser_target)
    #                     print("声源点可能是单独的目标，其位置为(%s,%s)" % (laser_target_x, laser_target_y))
    #             elif radar_laser_min_error <= 2:
    #                 print("此时可能是位置为(%s,%s)的雷达目标在说话" % (radar_x[min_dis_radar_id], radar_y[min_dis_radar_id]))
    #                 print("声源点与雷达相近，其坐标点可能为(%s,%s)" % (laser_x[min_dis_laser_id], laser_y[min_dis_laser_id]))
    #                 aver_fusion_x = (radar_x[min_dis_radar_id] + laser_x[min_dis_laser_id]) / 2.0
    #                 if aver_fusion_x > -7 and aver_fusion_x < -3:
    #                     fusion_target_x = (2 * radar_x.pop(min_dis_radar_id) + laser_x.pop(min_dis_laser_id)) / 3
    #                     fusion_target_y = (2 * radar_y.pop(min_dis_radar_id) + laser_y.pop(min_dis_laser_id)) / 3
    #                     fusion_target = (fusion_target_x, fusion_target_y)
    #                     fusion_list.append(fusion_target)
    #                 elif aver_fusion_x > -3 and aver_fusion_x < 3:
    #                     fusion_target_x = (radar_x.pop(min_dis_radar_id) + laser_x.pop(min_dis_laser_id)) / 2
    #                     fusion_target_y = (radar_y.pop(min_dis_radar_id) + laser_y.pop(min_dis_laser_id)) / 2
    #                     fusion_target = (fusion_target_x, fusion_target_y)
    #                     fusion_list.append(fusion_target)
    #                 elif aver_fusion_x > 3 and aver_fusion_x < 7:
    #                     fusion_target_x = (4 * radar_x.pop(min_dis_radar_id) + 0 * laser_x.pop(min_dis_laser_id)) / 4
    #                     fusion_target_y = (4 * radar_y.pop(min_dis_radar_id) + 0 * laser_y.pop(min_dis_laser_id)) / 4
    #                     fusion_target = (fusion_target_x, fusion_target_y)
    #                     fusion_list.append(fusion_target)
    #             for m in range(len(radar_x)):
    #                 target = (radar_x[m], radar_y[m])
    #                 fusion_list.append(target)
    #             print("此次融合的雷达数据为%s，声源数据为%s" % (radar, laser))
    #             print("全部融合后各目标位置为", fusion_list)
    #             for i in range(len(fusion_list)):
    #                 object_radar_x = format(fusion_list[i][0], '.3f')
    #                 object_radar_y = format(fusion_list[i][1], '.3f')
    #                 msg = msg + '(' + object_radar_x + ',' + object_radar_y + ')'
    #
    #         self.signal_fusion_flush.emit(fusion_list)
    #         self.history_fusion_point(fusion_list,self.fusion_history_point_list)
    #         msg = msg + '\n'
    #         self.signal_write_fusion_msg.emit(msg)

    def history_fusion_point(self, fusion_list,history_point_list):
        if len(history_point_list) == 0:
            for i in range(0, len(fusion_list)):
                history_point_list.append([])
                history_point_list[i].append(fusion_list[i])
                history_point_list[i].append(fusion_list[i])
        else:
            num = len(history_point_list)

            for i in range(0, len(fusion_list)):
                point_x = fusion_list[i][0]
                point_y = fusion_list[i][1]
                index = 0
                distance_min = 10
                list_distance = []
                for j in range(0, num):
                    x = history_point_list[j][0][0]
                    y = history_point_list[j][0][1]
                    distance = ((float(point_x) - float(x)) ** 2 + (float(point_y) - float(y)) ** 2) ** 0.5
                    list_distance.append(distance)
                    if distance < distance_min:
                        index = j
                        distance_min = distance
                distance_up_1_num = 0
                for distance in list_distance:
                    if distance > 1:
                        distance_up_1_num = distance_up_1_num + 1
                if distance_up_1_num == len(list_distance):
                    history_point_list.append([])
                    history_point_list[len(history_point_list) - 1].append(fusion_list[i])
                    history_point_list[len(history_point_list) - 1].append(fusion_list[i])
                    num = len(history_point_list)  # 更新列表元素
                else:
                    sum_x = (history_point_list[index][0][0] * (
                                len(history_point_list[index]) - 1) + point_x) / len(
                        history_point_list[index])
                    sum_y = (history_point_list[index][0][1] * (
                                len(history_point_list[index]) - 1) + point_y) / len(
                        history_point_list[index])
                    history_point_list[index][0] = (sum_x, sum_y)
                    history_point_list[index].append(fusion_list[i])

    # 雷达墙体数据解析
    def dataRadarWallAnalysis(self,data_wall):
        xarray = []
        yarray = []
        data_wall_front_length = ''
        data_wall_front_width = ''
        max_len = len(data_wall)
        # print("max_len:", max_len)
        start = 0
        while start <= max_len:
            if (data_wall[start + 0:start + 5] == b'\xa5\x5a\xb5\x10\x31'):
                flag = 1
                data_wall_length_hex = data_wall[start + 5:start + 9]
                data_wall_length_dec = struct.unpack('i', bytes(data_wall_length_hex))[0]

                data_number_wall_dec = ''
                data_number_wall_time_dec = ''

                if ((sum(data_wall[2:-1]) & 0xff == data_wall[-1]) |flag ==1):  # 假设校验位为x2f
                    # 年
                    data_wall_time_year_hex = data_wall[start + 9:start + 11]
                    data_wall_time_year_dec = struct.unpack('h', bytes(data_wall_time_year_hex))[0]  #
                    # 月
                    data_wall_time_month_hex = data_wall[start + 11:start + 12]
                    data_wall_time_month_dec = struct.unpack('b', bytes(data_wall_time_month_hex))[0]  #
                    # 日
                    data_wall_time_day_hex = data_wall[start + 12:start + 13]
                    data_wall_time_day_dec = struct.unpack('b', bytes(data_wall_time_day_hex))[0]  #
                    # 时
                    data_wall_time_hour_hex = data_wall[start + 13:start + 14]
                    data_wall_time_hour_dec = struct.unpack('b', bytes(data_wall_time_hour_hex))[0]  #
                    # 分
                    data_wall_time_minute_hex = data_wall[start + 14:start + 15]
                    data_wall_time_minute_dec = struct.unpack('b', bytes(data_wall_time_minute_hex))[0]  #
                    # 秒
                    data_wall_time_second_hex = data_wall[start + 15:start + 16]
                    data_wall_time_second_dec = struct.unpack('b', bytes(data_wall_time_second_hex))[0]  #
                    # 毫秒
                    data_wall_time_millisecond_hex = data_wall[start + 16:start + 18]
                    data_wall_time_millisecond_dec = struct.unpack('h', bytes(data_wall_time_millisecond_hex))[0]  #
                    # 前墙X的最小值
                    data_wall_front_Xmin_hex = data_wall[start + 18:start + 22]
                    data_wall_front_Xmin_dec = struct.unpack('f', bytes(data_wall_front_Xmin_hex))[0]
                    data_house_front_length = format(data_wall_front_Xmin_dec, '.3f')
                    # 前墙X的最大值
                    data_wall_front_Xmax_hex = data_wall[start + 22:start + 26]
                    data_wall_front_Xmax_dec = struct.unpack('f', bytes(data_wall_front_Xmax_hex))[0]
                    data_house_front_width = format(data_wall_front_Xmax_dec, '.3f')
                    # 前墙Y的最小值
                    data_wall_front_Ymin_hex = data_wall[start + 26:start + 30]
                    data_wall_front_Ymin_dec = struct.unpack('f', bytes(data_wall_front_Ymin_hex))[0]
                    data_wall_front_length = format(data_wall_front_Ymin_dec, '.3f')
                    # 前墙Y的最大值
                    data_wall_front_Ymax_hex = data_wall[start + 30:start + 34]
                    data_wall_front_Ymax_dec = struct.unpack('f', bytes(data_wall_front_Ymax_hex))[0]
                    data_wall_front_width = format(data_wall_front_Ymax_dec, '.3f')
                    # 行
                    data_wall_row_hex = data_wall[start + 34:start + 38]
                    data_wall_row_dec = struct.unpack('I', bytes(data_wall_row_hex))[0]
                    # print("行", data_wall_row_dec)
                    # 列
                    data_wall_column_hex = data_wall[start + 38:start + 42]
                    data_wall_column_dec = struct.unpack('I', bytes(data_wall_column_hex))[0]
                    # print("列", data_wall_column_dec)
                    # 墙体数据
                    data_wall_hex = data_wall[start + 42:start + data_wall_length_dec + 9]

                    # print("len data_wall_hex:",len(data_wall_hex))
                    '写文件'
                    data_wall_time_dec = str(data_wall_time_year_dec) + '-' \
                                         + str(data_wall_time_month_dec) + '-' \
                                         + str(data_wall_time_day_dec) + '-' \
                                         + str(data_wall_time_hour_dec) + '-' \
                                         + str(data_wall_time_minute_dec) + '-' \
                                         + str(data_wall_time_second_dec) + '-' \
                                         + str(data_wall_time_millisecond_dec)
                    radar_name = "./data/Radar/" + "Radar-wall" + data_wall_time_dec + ".txt"
                    radar_data_file = open(radar_name, 'w')
                    radar_data_file.write(data_wall_hex.hex())
                    radar_data_file.close()

                    xarray, yarray = bin2img(data_wall_hex)
                else:
                    msg = '墙体数据错误\n'
                    self.signal_write_rader_msg.emit(msg)
            start = start + max_len + 1
        self.signal_wall_rader_flush.emit(xarray, yarray,data_wall_front_length,data_wall_front_width,data_house_front_length,data_house_front_width)

if __name__ == '__main__':
    app = QtWidgets.QApplication(sys.argv)
    ui = TcpLogic(1)
    ui.show()
    sys.exit(app.exec_())
